A robot unknown environment exploration method based on adaptive KMeans algorithm

Abstract

The invention discloses a robot unknown environment exploration method based on an adaptive KMeans algorithm. An unknown area in which a robot moves in an unknown environment is obtained, and the unknown area is identified based on an RRT algorithm to obtain boundary points. The KMeans algorithm is used to cluster the boundary points, and the boundary points after clustering are calculated based on an information increment algorithm to determine a target point. The robot is guided to reach the target point based on a TEB path planning algorithm, and a Gmapping algorithm is used to construct a map to complete the robot unknown environment exploration. The number of clustering centers can be dynamically adjusted according to the boundary points collected from the map, and the optimal number of clustering centers can be intelligently determined according to the characteristics and distribution of data, thereby improving the effect and accuracy of clustering. The boundary points are clustered based on the KMeans clustering algorithm, and the target points are optimized through the information increment algorithm to improve the accuracy and efficiency of clustering.

Description

Technical Field

[0001] The invention belongs to the technical field of mobile robot autonomous navigation, and in particular relates to a robot unknown environment exploration method based on an adaptive KMeans algorithm. Background Art

[0002] In recent years, with the continuous improvement of hardware equipment, mobile robots have been widely used in various fields such as industry, medical care, agriculture, and service industry. Autonomous exploration and navigation is an important branch in the field of mobile robot research, which involves the exploration of robots in unknown environments. Robotic exploration of unknown environments aims to guide robots into unknown spaces. Among the research results in the field of robot exploration, the boundary exploration algorithm based on RRT (Rapidly exploring Random Tree) is the most widely used method. This exploration strategy consists of three modules: boundary detector module, filtering module, and robot task allocator module. The boundary detector module uses the RRT algorithm to detect boundary points and passes a large number of detected boundary points to the filtering module. The filtering module clusters the boundary points using the Mean Shift clustering algorithm. The filtering module is responsible for deleting invalid and outdated boundary points. Finally, the task allocation module selects the optimal boundary points through information increment calculation and assigns them to the mobile robot for exploration.

[0003] The boundary detector module based on the RRT algorithm identifies boundary points by constructing an exploration tree. A boundary point is defined as follows: if a point is located in an unknown area of ​​the map and is reached by the growth of the RRT tree, then the point is considered a boundary point. Such a definition helps the robot explore unknown environments more effectively and identify their boundaries. The detected boundary points will be processed by the filtering module, in which a clustering algorithm will be used. The Mean Shift clustering algorithm is used in traditional exploration. The Mean Shift algorithm is a density-based non-parametric clustering algorithm. Its core idea is to assume that data sets of different clusters have different probability density distributions, find the fastest direction of density increase for any sample point, and the area with high sample density corresponds to the maximum value of the distribution. These sample points will eventually converge to the maximum value of the local density, and the points that converge to the same local maximum value are considered to be members of the same cluster. The Mean Shift algorithm can handle clustering problems of arbitrary shapes and is widely used in image processing and analysis as well as computer vision technology, such as image denoising, image segmentation, motion tracking, etc. The Mean Shift algorithm has a small amount of computation and lacks a dynamic update mechanism. Due to the fixed bandwidth diameter, the clustering effect will be incomplete when the target size changes, and the clustering effect will be poor when the target speed increases. Summary of the invention

[0004] In view of this, the present invention provides a robot unknown environment exploration method based on an adaptive KMeans algorithm. It is found in the exploration of unknown environments of mobile robots that the original Mean Shift algorithm can be replaced by the KMeans algorithm. The KMeans algorithm is a commonly used clustering algorithm for dividing data points into K clusters. The basic idea of ​​the KMeans algorithm is to update the mean of all data points in the cluster as the centroid through continuous iteration, and use the mean of all data points in the cluster as the new centroid until the convergence condition is reached. The KMeans algorithm has the advantages of being simple to understand, high in computational efficiency and easy to implement, and is specifically implemented by the following technical solutions.

[0005] The present invention provides a robot unknown environment exploration method based on an adaptive KMeans algorithm, comprising the following steps:

[0006] Obtaining an unknown area where the robot moves in an unknown environment, and identifying the unknown area based on the RRT algorithm to obtain boundary points;

[0007] The boundary points are clustered using the KMeans algorithm, and the boundary points after clustering are calculated based on the information increment algorithm to determine the target point;

[0008] The robot is guided to reach the target point based on the TEB path planning algorithm, and the Gmapping algorithm is used to construct a map to complete the robot's exploration of an unknown environment.

[0009] As a preferred embodiment of the above technical solution, identifying the unknown area based on the RRT algorithm to obtain boundary points includes:

[0010] When applying the RRT algorithm to explore an unknown environment, the robot's current position is set as the root node of the exploration tree. , the RRT algorithm generates random points in the exploration tree , and determine the random point The nearest nearby point ;

[0011] Set the step size to , and calculate the random point and nearby points The slope m between:

[0012] (1)

[0013] In the formula, Represents random points The y coordinate of Indicates the nearby point The y coordinate of Represents random points The x-coordinate of Represents random points The x coordinate of

[0014] When random points With nearby points When they do not overlap, according to the step size, random points With nearby points The position and slope m determine the new node The coordinate position of To a random point The direction of the new node is determined with the step parameter as the distance The coordinate position of , the corresponding expression is:

[0015] (2)

[0016] (3)

[0017] In the formula, S is used to determine the relationship between two points. Greater than , S is positive; if Less than , S is negative; if equal , S is zero, For new nodes The x-coordinate of For new nodes The y coordinate of

[0018] When random points With nearby points When they overlap, the position of the new node will be recalculated, and the corresponding expression is:

[0019] (4)

[0020] （5）

[0021] In the formula, and A new node when a random point coincides with a nearby point The coordinates of .

[0022] As a preferred embodiment of the above technical solution, by calculating the Euclidean distance between two points and dynamically adjusting the step size to adapt to the exploration needs of different areas, the known vector is preset. and vector And calculate the Euclidean distance The expression is:

[0023] (6)

[0024] According to the new node Determine the new node Whether the line between the new node and the adjacent point passes through obstacles or unexplored areas to evaluate the new node The corresponding path;

[0025] If a new node is detected If the line between the new node and the adjacent point passes through the unknown area, the new node Mark as boundary point;

[0026] If not, the RRT algorithm will allow the exploration tree to grow and record the grown path in the exploration tree.

[0027] As a preferred embodiment of the above technical solution, the explored boundary points are managed, including:

[0028] Clear old boundary points: Before each exploration, clear the boundary points that have been explored in the previous sampling;

[0029] Cluster center point evaluation: Use the KMeans clustering algorithm to obtain the cluster center points of the boundary points and determine whether the cluster center points have been explored;

[0030] If so, remove the cluster center point from the cluster center point set corresponding to the cluster center point;

[0031] Among them, all explored boundary points are processed by KMeans clustering algorithm, which includes: selecting K centroids and initializing the centroids, calculating the distance between each boundary point and all centroids, and classifying each boundary point into the cluster represented by the nearest centroid; recalculating the centroid for each formed cluster, that is, taking the mean of all sample points in the cluster as the new centroid position;

[0032] The similarity of sample points within a cluster is evaluated by the sum of the distances from all boundary points to the centroid. The smaller the sum of distances, the higher the similarity of sample points within the cluster and the smaller the difference.

[0033] Presets is the i-th point on the K-th cluster, is the centroid of the Kth cluster, Represents the distance from the sample point to the centroid, and the corresponding distance measurement expression is:

[0034] (7)

[0035] In the formula, m is the number of clusters, and K is the number of cluster categories;

[0036] All points in a cluster are divided according to the Euclidean distance To the centroid The sum of squared distances The expression is:

[0037] (8)

[0038] Where n is the number of data points in the Kth cluster;

[0039] The KMeans algorithm is based on the principle of minimum square error and is used to minimize the sum of the squares of the distances from all points to the centroid in each cluster. The corresponding square error It can be expressed as:

[0040] (9)

[0041] In the formula, is the ith cluster, is the centroid of the Kth cluster, is the i-th data point in the cluster, d represents the Euclidean distance, Represents all points in the cluster The mean of :

[0042] (10)

[0043] In the formula, is the number of points in the K clusters. By calculating the sum of the squared distances from all points in each cluster to the centroid, the KMeans algorithm iterates continuously and updates the position of the centroid to minimize the total squared error of the entire data set.

[0044] As a preferred embodiment of the above technical solution, a KMeans algorithm is used to cluster the boundary points, including:

[0045] Obtain cluster center points of boundary points, evaluate the cluster center points on the cost map and allocate target points, wherein priority is set for boundary points according to preset evaluation criteria, and information entropy of each boundary point is calculated to evaluate its exploration value, wherein the calculation criteria include selecting appropriate navigation target points according to the calculated information entropy and arranging the robot to explore the location area;

[0046] Determine the navigation target point based on information entropy. Information entropy is the expected value of the amount of information to describe uncertainty. :

[0047] (11)

[0048] In the formula, s is the sample space, is the probability of i occurring;

[0049] Select the navigation target point through the profit formula of the boundary point:

[0050] (12)

[0051] In the formula, is a given frontier point, R represents the revenue of the frontier point, I represents the information increment of the frontier point, is the current location of the robot, is the parameter given by the user, N is the navigation cost of the robot to navigate to the frontier point, The hysteresis gain is used to determine the given leading edge point Is it within the robot's current position? Certain radius In addition, if The value of is equal to 1; otherwise, The value is equal to ,in, Greater than 1; by setting If it is greater than 1, the robot is more inclined to explore the frontier points near it. The corresponding expression is:

[0052] (13)

[0053] In the formula, The radius is based on the current robot.

[0054] As a preferred embodiment of the above technical solution, guiding the robot to reach the target point based on the TEB path planning algorithm also includes:

[0055] The optimal target point is determined according to the frontier point, and the key target point is used as the navigation target point in the local path planning of the TEB algorithm. The corresponding robot posture expression is:

[0056] (14)

[0057] In the formula, , and is the position and posture of the robot in the map coordinate system, is the state space in the time dimension and the control parameter dimension, indicates that the positions in the coordinate system are all positive real numbers, Indicates all possible status conditions. represents the Cartesian product;

[0058] The position and posture of the robot in the map coordinate system are taken as the configuration, and the configuration in space can be sorted as follows:

[0059] (15)

[0060] There is a time interval between the two configurations, which is defined as , represents the time required for the two configurations of the robot, and the expression of the sequence recording the time is:

[0061] (16)

[0062] The configuration and time series are combined as:

[0063] (17)

[0064] The optimal path point, that is, the optimal Q, is obtained through weighted multi-objective optimization. The corresponding expression is:

[0065] (18)

[0066] (19)

[0067] In the formula, B is a subset of the arrangement sorting, which is used to evaluate and optimize to obtain the optimal solution; For the best result; is the objective function of the various constraints considered, and is the component of each objective function The weighted sum of is the number of objective functions.

[0068] As a preferred embodiment of the above technical solution, the first constraint objective function of the TEB algorithm is the path following and obstacle avoidance constraints. The obstacle avoidance constraints include following the known global planning path and avoiding obstacles, and the configuration sequence is consistent with the global path point sequence or obstacle. The closest distance is , follow the path target to configure the maximum allowed distance from the global path As a constraint, the obstacle avoidance target is configured to have the minimum distance allowed from the obstacle. as a constraint;

[0069] Constraining functions based on paths and barrier constraint functions With penalty function , the corresponding expression is:

[0070] (20)

[0071] In the formula, represents a small displacement next to the limit value, S represents scaling, and n represents the polynomial order.

[0072] As a preferred embodiment of the above technical solution, the second constraint objective function of the TEB algorithm is the speed and acceleration constraint. The average linear velocity and angular velocity of the robot movement can be obtained by adjacent , and time interval The calculation results are:

[0073] (twenty one)

[0074] (twenty two)

[0075] In the formula, and is the distance between two points, is the rotation angle between two points;

[0076] The calculation expressions of the robot's average linear acceleration and angular velocity are:

[0077] (twenty three)

[0078] Line speed constraints Available penalty functions It is expressed as:

[0079] (twenty four)

[0080] The upper and lower bounds of the constraints are determined by combining the upper and lower bounds of the wheel speed and acceleration obtained for the mobile robot, where: Indicates the line speed, Indicates the maximum line speed, represents a small displacement next to the limit value, S represents scaling, and n represents the polynomial order;

[0081] The motion state of the robot chassis is defined by three key variables: linear movement along the x-axis and y-axis and rotation. The rotation of the axis, Refers to the rotation around the z-axis; decompose the robot chassis motion into three independent variables with the robot's four wheels as the geometric center: represents the x-axis translation, represents the x-axis translation, express The rotation speed of the shaft, the axis speed of the wheel is obtained as:

[0082] (25)

[0083] In the formula, is the vector from the geometric center to the wheel axis, is the velocity vector of the wheel axis, The wheel axis is vertical The velocity vector in the direction of the wheel axis is calculated as follows:

[0084] (26)

[0085] (27)

[0086] The speed of the roller can be calculated based on the speed of the wheel axis, which can be decomposed into the speed along the roller direction. and the speed perpendicular to the roller direction , where the vertical direction can be ignored, and the corresponding expression is:

[0087] (28)

[0088] in, is the unit vector along the roller direction, so the wheel velocity v is calculated:

[0089] (29).

[0090] As a preferred embodiment of the above technical solution, the third constraint objective function of the TEB algorithm is a kinematic constraint. The robot moves in a smooth trajectory composed of several arc segments. With movement direction , Angle The angle that should correspond to the end configuration Equal, if is the absolute posture of the robot at the i-th arc segment relative to the world coordinate system, then:

[0091] (30)

[0092] Among them, the displacement vector in the direction of motion is expressed as:

[0093] (31)

[0094] In the formula, and Respectively represent the displacement of the coordinates of the two points on the path, and the corresponding objective function :

[0095] (32)

[0096] In the formula, and Respectively represent the coordinates of two points on the path. Formula (32) represents the direction of the two points on the path by adding the two vectors; multiplying by the displacement vector , , represents the distance and direction between two points on the path; is the square norm of the vector, which represents the cost function of the path.

[0097] As a preferred embodiment of the above technical solution, the fourth constraint objective function of the TEB algorithm is the fastest path constraint, which is expressed by minimizing the quadratic power of the time interval sequence:

[0098] (33)

[0099] When performing TEB path planning, the Gmapping algorithm is used simultaneously to create a map of the unknown environment. The algorithm constructs a grid map by dividing the two-dimensional space into multiple independent grid cells. Each grid cell is associated with two probability values: represents the probability that the grid cell is a traversable space, Represents the probability of an obstacle occupying the grid; the sum of the two probability values ​​is 1, and the state of the grid cell can be determined by the ratio of the two probability values. To determine, a higher ratio indicates that the grid cell may be an obstacle;

[0100] When a specific point in the map receives a new measurement value z, the state needs to be updated accordingly; it is assumed that before the new measurement value arrives, the state of the specific point is known and it is updated to the new estimated state :

[0101] (34)

[0102] In the formula, represents the log probability of state s, represents the probability that state s is 1 under given condition z; It represents the probability that state s is 0 under given condition z;

[0103] According to the Bayesian formula, we can get:

[0104] (35)

[0105] And take the logarithm of both sides of formula (35):

[0106] (36)

[0107] In the initial state without any measurement value, the initial state of a point is 0, and the ratio is calculated in formula (34) as the model of the measurement value;

[0108] In fact, there are two models of measurement values:

[0109] (37)

[0110] In the formula, both measurements follow a fixed model, so that each time a new measurement is obtained, the state of the point on the map can be updated through simple arithmetic addition and subtraction operations;

[0111] When constructing a grid map, the robot converts the real-world position into grid coordinates on the map. The length of each grid unit in the map is represented by r. Indicates the resolution of the map; for a two-dimensional grid map, the position of any point can be used to coordinate The coordinates represent the horizontal and vertical distances relative to the origin of the map, where each grid position coordinate is determined by express:

[0112] (38)

[0113] In the formula, x and y represent the coordinates in the real world, i and j are the coordinates in the grid map, and r is the length of one grid cell. represents the resolution, then and ;

[0114] The default robot posture is , the location of obstacles in the map construction process can be obtained :

[0115] (39)

[0116] (40)

[0117] Where d is the measured distance, is the angle between the laser line and the robot's posture angle. After obtaining the two coordinates, the grid positions of the two points are calculated. and , use the Bresenham algorithm to calculate the set of non-obstacle grid points and update the grid map;

[0118] The coordinates of the i-th pixel point on the preset straight line , the slope of the straight line is 1 and , the possible position of the next pixel on the line is or ,exist The y value of the point on the line is , the distance between the point on the straight line and the pixel point and pixels The distances are and :

[0119] (41)

[0120] (42)

[0121] In the formula, b is the slope of the straight line equation;

[0122] and The distance difference is:

[0123] (43)

[0124] The distance difference can be used to select the next pixel. When the distance difference is positive, , indicating that the theoretical point on the straight line The pixel is closer, the next pixel is taken ; When the distance difference is negative, , indicating that the theoretical point on the straight line is The pixel is closer, then the next pixel is ; When the distance difference is zero, take as the next pixel.

[0125] The invention provides a robot unknown environment exploration method based on an adaptive KMeans algorithm. An unknown area in which a robot moves in an unknown environment is obtained, and the unknown area is identified based on an RRT algorithm to obtain boundary points. The KMeans algorithm is used to cluster the boundary points, and the boundary points after clustering are calculated based on an information increment algorithm to determine a target point. The robot is guided to reach the target point based on a TEB path planning algorithm, and a Gmapping algorithm is used to construct a map to complete the robot unknown environment exploration. The number of clustering centers can be dynamically adjusted according to the boundary points collected from the map, and the optimal number of clustering centers can be intelligently determined according to the characteristics and distribution of data, thereby improving the effect and accuracy of clustering. The boundary points are clustered based on the KMeans clustering algorithm, and the target points are optimized through the information increment algorithm to improve the accuracy and efficiency of clustering. BRIEF DESCRIPTION OF THE DRAWINGS

[0126] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0127] Figure 1 A flowchart of a robot unknown environment exploration method based on an adaptive KMeans algorithm provided by the present invention;

[0128] Figure 2 A map construction process diagram of a first unknown environment provided by the present invention;

[0129] Figure 3 A diagram of the map construction process of the second unknown environment provided by the present invention. DETAILED DESCRIPTION

[0130] Embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and cannot be understood as limiting the present invention.

[0131] The present invention aims to effectively solve several key problems in unknown environment exploration, including boundary point exploration, clustering, allocation, target navigation, and ensuring the integrity of map construction. A robot unknown environment exploration method combining KMeans clustering algorithm is provided, which integrates KMeans algorithm on the basis of RRT algorithm.

[0132] See also Figure 1 The present invention provides a robot unknown environment exploration method based on an adaptive KMeans algorithm, comprising the following steps:

[0133] S1: Obtain an unknown area where the robot moves in an unknown environment, and identify the unknown area based on the RRT algorithm to obtain boundary points;

[0134] S2: clustering the boundary points using the KMeans algorithm, and calculating the boundary points after clustering based on the information increment algorithm to determine the target point;

[0135] S3: Guide the robot to the target point based on the TEB path planning algorithm, and use the Gmapping algorithm to build a map to complete the robot's exploration of an unknown environment.

[0136] In this embodiment, the RRT algorithm is responsible for identifying the map boundary points as the exploration targets of the mobile robot in this technology. The KMeans algorithm performs clustering processing on all boundary points found during the RRT exploration process. The processed boundary points are calculated using information increment to determine the key target points, and the robot is guided to these target points through the TEB (Time Elastic Band) path planning algorithm. As for map construction, the Gmapping algorithm is used to implement it.

[0137] The unknown area is identified based on the RRT algorithm to obtain boundary points, including:

[0138] When applying the RRT algorithm to explore an unknown environment, the robot's current position is set as the root node of the exploration tree. , the RRT algorithm generates random points in the exploration tree , and determine the random point The nearest point ;

[0139] Set the step size to , and calculate the random point and nearby points The slope m between:

[0140] (1)

[0141] In the formula, Represents random points The y coordinate of Indicates the nearby point The y coordinate of Represents random points The x-coordinate of Represents random points The x coordinate of

[0142] When random points With nearby points When they do not overlap, according to the step size, random points With nearby points The position and slope m determine the new node The coordinate position of To a random point The direction of the new node is determined with the step parameter as the distance The coordinate position of , the corresponding expression is:

[0143] (2)

[0144] (3)

[0145] In the formula, S is used to determine the relationship between two points. Greater than , S is positive; if Less than , S is negative; if equal , S is zero, For new nodes The x-coordinate of For new nodes The y coordinate of

[0146] When random points With nearby points When they overlap, the position of the new node will be recalculated, and the corresponding expression is:

[0147] (4)

[0148] （5）

[0149] In the formula, and A new node when a random point coincides with a nearby point The coordinates of .

[0150] It should be noted that the step size is defined as , It is a parameter between 0 and a certain maximum value. The larger the step size, the faster the exploration tree grows, which helps to quickly expand the exploration range. However, too large a step size may reduce the exploration accuracy of map details, thereby affecting the exploration integrity of the map. Therefore, a reasonable choice of step size is the key to balancing the exploration speed and map integrity. In RRT, it is necessary to find the node closest to the random point in the exploration tree. The Euclidean distance is the standard for measuring this "closest". By calculating the Euclidean distance between points, the step size can also be dynamically adjusted to meet the exploration needs of different areas. The robot unknown environment exploration process based on the adaptive KMeans algorithm of the present invention is: using boundary point exploration based on the RRT algorithm to identify unknown areas in an unknown environment; clustering the boundary points based on the KMeans clustering algorithm, and optimizing the target navigation allocation points through the information increment algorithm to improve the accuracy and efficiency of clustering; in the target navigation stage of map exploration, the TEB local path planning algorithm is used to ensure that the robot can navigate to the target point along the optimal path, and the Gammping algorithm is used for map construction.

[0151] It should be understood that the adaptive KMeans algorithm is an intelligent clustering technology that can dynamically adjust the number of cluster centers according to the boundary points collected by the map. The core advantage of this algorithm lies in its adaptive ability, that is, it can intelligently determine the optimal number of cluster centers according to the characteristics and distribution of the data, thereby improving the effect and accuracy of clustering. By obtaining the unknown area where the robot moves in the unknown environment, and identifying the unknown area based on the RRT algorithm to obtain the boundary points, the KMeans algorithm is used to cluster the boundary points, and the boundary points after clustering are calculated based on the information increment algorithm to determine the target point, the robot is guided to the target point based on the TEB path planning algorithm, and the Gmapping algorithm is used to build the map to complete the robot's unknown environment exploration. The number of cluster centers can be dynamically adjusted according to the boundary points collected by the map, and the optimal number of cluster centers can be intelligently determined according to the characteristics and distribution of the data, thereby improving the effect and accuracy of clustering, clustering the boundary points based on the KMeans clustering algorithm, and optimizing the target points through the information increment algorithm to improve the accuracy and efficiency of clustering.

[0152] Optionally, by calculating the Euclidean distance between two points and dynamically adjusting the step size to adapt to the exploration needs of different areas, the known vectors are preset and vector And calculate the Euclidean distance The expression is:

[0153] (6)

[0154] According to the new node Determine the new node and the adjacent points Whether the connection passes through obstacles or unexplored areas to evaluate the path corresponding to the new node;

[0155] If a new node is detected If the line between the new node and the adjacent point passes through the unknown area, the new node Mark as boundary point;

[0156] If not, the RRT algorithm will allow the exploration tree to grow and record the grown path in the exploration tree.

[0157] In this embodiment, the explored boundary points are managed, including:

[0158] Clear old boundary points: Before each exploration, clear the boundary points that have been explored in the previous sampling;

[0159] Cluster center point evaluation: Use the KMeans clustering algorithm to obtain the cluster center points of the boundary points and determine whether the cluster center points have been explored;

[0160] If so, remove the cluster center point from the cluster center point set corresponding to the cluster center point;

[0161] Among them, all explored boundary points are processed by KMeans clustering algorithm, which includes: selecting K centroids and initializing the centroids, calculating the distance between each boundary point and all centroids, and classifying each boundary point into the cluster represented by the nearest centroid; recalculating the centroid for each formed cluster, that is, taking the mean of all sample points in the cluster as the new centroid position;

[0162] The similarity of sample points within a cluster is evaluated by the sum of the distances from all boundary points to the centroid. The smaller the sum of distances, the higher the similarity of sample points within the cluster and the smaller the difference.

[0163] Presets is the i-th point on the K-th cluster, is the centroid of the Kth cluster, Represents the distance from the sample point to the centroid, and the corresponding distance measurement expression is:

[0164] (7)

[0165] In the formula, m is the number of clusters, and K is the number of cluster categories;

[0166] All points in a cluster are divided according to the Euclidean distance To the centroid The sum of squared distances The expression is:

[0167] (8)

[0168] Where n is the number of data points in the Kth cluster;

[0169] The KMeans algorithm is based on the principle of minimum square error and is used to minimize the sum of the squares of the distances from all points to the centroid in each cluster. The corresponding square error It can be expressed as:

[0170] (9)

[0171] In the formula, is the ith cluster, is the centroid of the Kth cluster, is the i-th data point in the cluster, d represents the Euclidean distance, Represents all points in the cluster The mean of :

[0172] (10)

[0173] In the formula, is the number of points in the K clusters. By calculating the sum of the squared distances from all points in each cluster to the centroid, the KMeans algorithm iterates and updates the position of the centroid to minimize the total squared error of the entire data set. In other words, by calculating the sum of the squared distances from all points in each cluster to the centroid, the KMeans algorithm iterates and updates the position of the centroid to minimize the total squared error of the entire data set.

[0174] It should be noted that the KMeans algorithm processes the distribution of points and the updating of the centroid, which is repeated until the centroid position no longer changes significantly, indicating that the algorithm has converged. Through this process, the boundary points are effectively grouped into different clusters, providing structured and valuable information for the robot's further exploration and path planning.

[0175] Optionally, clustering the boundary points using a KMeans algorithm includes:

[0176] Obtain cluster center points of boundary points, evaluate the cluster center points on the cost map and allocate target points, wherein priority is set for boundary points according to preset evaluation criteria, and information entropy of each boundary point is calculated to evaluate its exploration value, wherein the calculation criteria include selecting appropriate navigation target points according to the calculated information entropy and arranging the robot to explore the location area;

[0177] Determine the navigation target point based on information entropy. Information entropy is the expected value of the amount of information to describe uncertainty. :

[0178] (11)

[0179] In the formula, s is the sample space, is the probability of i occurring;

[0180] Select the navigation target point through the profit formula of the boundary point:

[0181] (12)

[0182] In the formula, is a given frontier point, R represents the revenue of the frontier point, I represents the information increment of the frontier point, is the current location of the robot, is the parameter given by the user, N is the navigation cost of the robot to navigate to the frontier point, The hysteresis gain is used to determine the given leading edge point Is it within the robot's current position? Certain radius In addition, if The value of is equal to 1; otherwise, The value is equal to ,in, Greater than 1; by setting If it is greater than 1, the robot is more inclined to explore the frontier points near it. The corresponding expression is:

[0183] (13)

[0184] In the formula, The radius is based on the current robot.

[0185] In this embodiment, by setting When it is greater than 1, the robot is more inclined to explore the frontier points near it, which helps to avoid the overlap of frontier points.

[0186] Optionally, guiding the robot to reach the target point based on the TEB path planning algorithm further includes:

[0187] The optimal target point is determined according to the frontier point, and the key target point is used as the navigation target point in the local path planning of the TEB algorithm. The corresponding robot posture expression is:

[0188] (14)

[0189] In the formula, , and is the position and posture of the robot in the map coordinate system, is the state space in the time dimension and the control parameter dimension, indicates that the positions in the coordinate system are all positive real numbers, Indicates all possible status conditions. represents the Cartesian product; in other words, determine the optimal target point, which will be used as the navigation target of the TEB algorithm in local path planning. The principle of the TEB algorithm is similar to an elastic belt, which starts from the location where the robot is exploring, extends to a given target point, and can deform to adapt to the external force of the environment while meeting specific constraints. A mobile robot equipped with Mecanum wheels is used for precise control and path planning;

[0190] The position and posture of the robot in the map coordinate system are taken as the configuration, and the configuration in space can be sorted as follows:

[0191] (15)

[0192] There is a time interval between the two configurations, which is defined as , represents the time required for the two configurations of the robot, and the expression of the sequence recording the time is:

[0193] (16)

[0194] The configuration and time series are combined as:

[0195] (17)

[0196] The optimal path point, that is, the optimal Q, is obtained through weighted multi-objective optimization. The corresponding expression is:

[0197] (18)

[0198] (19)

[0199] In the formula, B is a subset of the arrangement sorting, which is used to evaluate and optimize to obtain the optimal solution; For the best result; is the objective function of the various constraints considered, and is the component of each objective function The weighted sum of is the number of objective functions. Each objective function is only related to a few continuous states in the path elastic band, not the entire path, so the optimization is the optimization of the sparse matrix model.

[0200] In this embodiment, the first constraint objective function of the TEB algorithm is the path following and obstacle avoidance constraints. The obstacle avoidance constraints include following the known global planning path and avoiding obstacles. The configuration sequence is consistent with the global path point sequence or obstacle. The closest distance is , follow the path target to configure the maximum allowed distance from the global path As a constraint, the obstacle avoidance target is configured to have the minimum distance allowed from the obstacle. as a constraint;

[0201] Constraining functions based on paths and barrier constraint functions With penalty function , the corresponding expression is:

[0202] (20)

[0203] In the formula, represents a small displacement next to the limit value, S represents scaling, and n represents the polynomial order.

[0204] Optionally, the second constraint objective function of the TEB algorithm is the velocity and acceleration constraint. The average linear velocity and angular velocity of the robot motion can be obtained by the adjacent , and time interval The calculation results are:

[0205] (twenty one)

[0206] (twenty two)

[0207] In the formula, and is the distance between two points, is the rotation angle between two points;

[0208] The calculation expressions of the robot's average linear acceleration and angular velocity are:

[0209] (twenty three)

[0210] Line speed constraints Available penalty functions It is expressed as:

[0211] (twenty four)

[0212] The upper and lower bounds of the constraints are determined by combining the upper and lower bounds of the wheel speed and acceleration obtained for the mobile robot, where: Indicates the line speed, Indicates the maximum line speed, represents a small displacement next to the limit value, S represents scaling, and n represents the polynomial order;

[0213] The motion state of the robot chassis is defined by three key variables: linear movement along the x-axis and y-axis and rotation. The rotation of the axis, Refers to the rotation around the z-axis; decompose the robot chassis motion into three independent variables with the robot's four wheels as the geometric center: represents the x-axis translation, represents the x-axis translation, express The rotation speed of the shaft, the axis speed of the wheel is obtained as:

[0214] (25)

[0215] In the formula, is the vector from the geometric center to the wheel axis, is the velocity vector of the wheel axis, The wheel axis is vertical The velocity vector in the direction of the wheel axis is calculated as follows:

[0216] (26)

[0217] (27)

[0218] The speed of the roller can be calculated based on the speed of the wheel axis, which can be decomposed into the speed along the roller direction. and the speed perpendicular to the roller direction , where the vertical direction can be ignored, and the corresponding expression is:

[0219] (28)

[0220] in, is the unit vector along the roller direction, so the wheel velocity v is calculated:

[0221] (29).

[0222] In this embodiment, the robot is equipped with Mecanum wheels. This specially designed wheel gives the robot full range of motion. The motion state of the robot chassis is defined by three key variables: linear movement along the x-axis and y-axis and rotation around the robot. Rotation of the axis. Refers to the rotation around the Z axis, that is, the vertical axis. Considering the four-wheel drive characteristics of the robot, each wheel is driven by its corresponding motor, and there is a mutual constraint relationship between the speeds of these wheels. By analyzing these speed constraints, the inverse kinematics of the robot was successfully established, which is crucial for accurately controlling the movement of the robot. Among them, due to the particularity of the Mecanum wheel, the speed of the roller can be calculated according to the speed of the wheel axis, which is decomposed into the speed along the roller direction , and the speed perpendicular to the roller reverse direction , the vertical reversal can be ignored.

[0223] Optionally, the third constraint objective function of the TEB algorithm is a kinematic constraint. The robot moves in a smooth trajectory composed of several arc segments. With movement direction , Angle The angle that should correspond to the end configuration Equal, if is the absolute posture of the robot at the i-th arc segment relative to the world coordinate system, then:

[0224] (30)

[0225] Among them, the displacement vector in the direction of motion is expressed as:

[0226] (31)

[0227] In the formula, and Respectively represent the displacement of the coordinates of the two points on the path, and the corresponding objective function :

[0228] (32)

[0229] In the formula, and Respectively represent the coordinates of two points on the path. Formula (32) represents the direction of the two points on the path by adding the two vectors; multiplying by the displacement vector , , represents the distance and direction between two points on the path; is the square norm of the vector, which represents the cost function of the path.

[0230] Optionally, the fourth constraint objective function of the TEB algorithm is the fastest path constraint, which is expressed as minimizing the quadratic time interval sequence:

[0231] (33)

[0232] When performing TEB path planning, the Gmapping algorithm is used simultaneously to create a map of the unknown environment. The algorithm constructs a grid map by dividing the two-dimensional space into multiple independent grid cells. Each grid cell is associated with two probability values: represents the probability that the grid cell is a traversable space, Represents the probability of an obstacle occupying the grid; the sum of the two probability values ​​is 1, and the state of the grid cell can be determined by the ratio of the two probability values. To determine, a higher ratio indicates that the grid cell may be an obstacle;

[0233] When a specific point in the map receives a new measurement value z, the state needs to be updated accordingly; it is assumed that before the new measurement value arrives, the state of the specific point is known and it is updated to the new estimated state :

[0234] (34)

[0235] In the formula, represents the log probability of state s, represents the probability that state s is 1 under given condition z; It represents the probability that state s is 0 under given condition z;

[0236] According to the Bayesian formula, we can get:

[0237] (35)

[0238] And take the logarithm of both sides of formula (35):

[0239] (36)

[0240] In the initial state without any measurement value, the initial state of a point is 0, and the ratio is calculated in formula (34) as the model of the measurement value;

[0241] In fact, there are two models of measurement values:

[0242] (37)

[0243] In the formula, both measurements follow a fixed model, so that each time a new measurement is obtained, the state of the point on the map can be updated through simple arithmetic addition and subtraction operations, which effectively promotes the continuous updating of the map;

[0244] When constructing a grid map, the robot converts the real-world position into grid coordinates on the map. The length of each grid unit in the map is represented by r. Indicates the resolution of the map; for a two-dimensional grid map, the position of any point can be used to coordinate The coordinates represent the horizontal and vertical distances relative to the origin of the map, where each grid position coordinate is determined by express:

[0245] (38)

[0246] In the formula, x and y represent the coordinates in the real world, i and j are the coordinates in the grid map, and r is the length of one grid cell. represents the resolution, then and ;

[0247] The default robot posture is , the location of obstacles in the map construction process can be obtained :

[0248] (39)

[0249] (40)

[0250] Where d is the measured distance, is the angle between the laser line and the robot's posture angle. After obtaining the two coordinates, the grid positions of the two points are calculated. and , use the Bresenham algorithm to calculate the set of non-obstacle grid points and update the grid map; the Bresenham algorithm uses a recursive stepping method to step the coordinates of the direction of maximum change by one pixel each time, and at the same time, the coordinates of the other direction are determined based on the sign of the error discriminant to decide whether to step by one pixel as well.

[0251] Since the pixel points of the displayed straight line only take integer value coordinates, the coordinates of the i-th pixel point on the preset straight line , the slope of the straight line is 1 and , the possible position of the next pixel on the line is or ,exist The y value of the point on the line is , the distance between the point on the straight line and the pixel point and pixels The distances are and :

[0252] (41)

[0253] (42)

[0254] In the formula, b is the slope of the straight line equation;

[0255] and The distance difference is:

[0256] (43)

[0257] The distance difference can be used to select the next pixel. When the distance difference is positive, , indicating that the theoretical point on the straight line The pixel is closer, the next pixel is taken ; When the distance difference is negative, , indicating that the theoretical point on the straight line is The pixel is closer, then the next pixel is ; When the distance difference is zero, take as the next pixel.

[0258] Specifically, the method of the present invention can be used to conduct experiments in multiple environments. In the first experimental environment, i.e., the first unknown environment, Figure 2 As shown, a total of two experiments were conducted, and the map coverage rate was 100% in both explorations. The experimental results Figure 2The exploration time of (a) is 28.82s. The experimental results are shown in Figure 2. Figure 2 The exploration time of (b) is 29.88s. Figure 3 As shown in Figure 2, a total of three experiments were conducted in this environment. The results of the first experiment showed that the map coverage rate was 75% and the exploration time was 71.81s. Figure 3 As shown in (a) of the second experiment, the map coverage rate is 85% and the exploration time is 88.72s. Figure 3 As shown in (b) of the third experiment, the map coverage rate is 98% and the exploration time is 155.31s. Figure 3 As shown in (c) in .

[0259] Combined with the experimental results, the contents involved in the present invention are described in detail:

[0260] The adaptive KMeans algorithm is an intelligent clustering technology that can dynamically adjust the number of cluster centers according to the boundary points collected from the map. The core advantage of this algorithm lies in its adaptive ability, that is, it can intelligently determine the optimal number of cluster centers based on the characteristics and distribution of the data, thereby improving the effect and accuracy of clustering.

[0261] When exploring an unknown environment, the boundary points collected by the mobile robot through sensors are often complex and changeable. The adaptive KMeans algorithm first performs a preliminary analysis of these boundary points to evaluate the number of boundary points. Based on this preliminary analysis, the algorithm can identify how many cluster centers are needed. In order to effectively cover all boundary points, while maintaining the consistency within each cluster and the similarity between boundary points. During the experiment, the selection of K value also refers to the silhouette coefficient, which is an indicator for evaluating clustering effect. The best K value can be selected in the boundary point clustering. First, the calculation of the silhouette coefficient is based on the two concepts of intra-cluster similarity and inter-cluster similarity. The silhouette coefficient is analyzed by taking sample i as an example.

[0262] Intra-cluster similarity , represented by a cluster with i as the center point Average distance to other points in the cluster:

[0263] (44)

[0264] In the formula, It represents the sum of the distances from all points in the cluster to the cluster center, and n represents the number of points in the same cluster.

[0265] Inter-cluster similarity , represented as clusters The center boundary point i to the nearest cluster The average distance of all points in the cluster other than the one to which they belong:

[0266] (45)

[0267] In the formula, represents the sum of the distances of all points in the nearest cluster (not the cluster to which the cluster center belongs) to the cluster center, and n represents the number of points in the cluster to which the cluster center belongs. Different inter-cluster similarities can be expressed as , when k is equal to 1, express and The inter-cluster similarity between them, when k is equal to 2, express and The inter-cluster similarity between them is the smallest one, and the final inter-cluster similarity of point i is taken as:

[0268] (46)

[0269] In the formula, the value of k is the value that needs to be sampled for the silhouette coefficient.

[0270] The silhouette coefficient is obtained by obtaining the intra-cluster similarity and inter-cluster similarity. :

[0271] (47)

[0272] Analyze the similarity within the cluster and inter-cluster similarity The relationship between can be used to determine whether the cluster center point i is reasonable:

[0273] (49)

[0274] when Greater than When , it means that point i is close to the points in the cluster and far away from another cluster, and the silhouette coefficient tends to 1, indicating that the clustering of sample point i is reasonable; equal When , it means that the sample point i is between the two clusters and on the boundary of the two clusters, and the silhouette coefficient is 0, which is not reasonable; when Less than When , sample point i is scattered with the points in the cluster and is closer to another nearest cluster, indicating that sample i should be classified into another cluster.

[0275] Finally, by finding the mean s of all sample points, we get the silhouette coefficient of the clustering result:

[0276] (49)

[0277] In the above formula, n is the number of all sample points. The larger s is, the more reasonable the choice of K value is. The effectiveness of the adaptive KMeans algorithm is proved by multiple simulation results during the experiment:

[0278] The experimental parameters are as follows:

[0279] The experiment was carried out using the Gazebo simulation virtual environment in the ROS operating system. The computer operating system was Ubuntu 18.04, the CPU model was i5 8250U, quad-core / eight-thread, CPU main frequency was 1.6 GHz, the RAM was 8 GB DDR42400MHz, and the experimental robot was a mobile robot equipped with Mecanum wheels.

[0280] In all examples shown and described herein, any specific values ​​should be interpreted as merely exemplary and not as limiting, and thus other examples of the exemplary embodiments may have different values.

[0281] It should be noted that similar reference numerals and letters denote similar items in the following drawings, and therefore, once an item is defined in one drawing, further definition and explanation thereof is not required in subsequent drawings.

[0282] The above-mentioned embodiments only express several implementation methods of the present invention, and the description thereof is relatively specific and detailed, but it cannot be understood as limiting the scope of the present invention. It should be pointed out that for ordinary technicians in this field, several modifications and improvements can be made without departing from the concept of the present invention, which all belong to the protection scope of the present invention.

Claims

1. A robot unknown environment exploration method based on adaptive KMeans algorithm, characterized in that: The following steps are involved: Obtaining an unknown area where the robot moves in an unknown environment, and identifying the unknown area based on the RRT algorithm to obtain boundary points; The boundary points are clustered using the KMeans algorithm, and the boundary points after clustering are calculated based on the information increment algorithm to determine the target point; Guide the robot to the target point based on the TEB path planning algorithm, and use the Gmapping algorithm to build a map to complete the robot's exploration of an unknown environment; The step of identifying the unknown area based on the RRT algorithm to obtain boundary points includes: When applying the RRT algorithm to explore an unknown environment, the robot's current position is set as the root node of the exploration tree. , the RRT algorithm generates random points in the exploration tree , and determine the random point The nearest point ; Set the step size to , and calculate the random point and nearby points The slope m between: （1） In the formula, Represents random points The y coordinate of Indicates the nearby point The y coordinate of Represents random points The x-coordinate of Represents random points The x coordinate of When random points With nearby points When they do not overlap, according to the step size, random points With nearby points The position and slope m determine the new node The coordinate position of To a random point The direction of the new node is determined with the step parameter as the distance The coordinate position of , the corresponding expression is: （2） （3） In the formula, S is used to determine the relationship between two points. Greater than , S is positive; if Less than , S is negative; if equal , S is zero, For new nodes The x-coordinate of For new nodes The y coordinate of When random points With nearby points When they overlap, the position of the new node will be recalculated, and the corresponding expression is: （4） （5） In the formula, and A new node when a random point coincides with a nearby point The coordinates of By calculating the Euclidean distance between two points and dynamically adjusting the step size to adapt to the exploration needs of different areas, the known vectors are preset. and vector And calculate the Euclidean distance The expression is: （6） According to the new node Determine the new node Whether the line between the new node and the adjacent point passes through obstacles or unexplored areas to evaluate the new node The corresponding path; If a new node is detected If the line between the new node and the adjacent point passes through the unknown area, the new node Mark as boundary point; If not, the RRT algorithm will allow the exploration tree to grow and record the grown path in the exploration tree; Manage the explored boundary points, including: Clear old boundary points: Before each exploration, clear the boundary points that have been explored in the previous sampling; Cluster center point evaluation: Use the KMeans clustering algorithm to obtain the cluster center points of the boundary points and determine whether the cluster center points have been explored; If so, remove the cluster center point from the cluster center point set corresponding to the cluster center point; Among them, all explored boundary points are processed by KMeans clustering algorithm, which includes: selecting K centroids and initializing the centroids, calculating the distance between each boundary point and all centroids, and classifying each boundary point into the cluster represented by the nearest centroid; recalculating the centroid for each formed cluster, that is, taking the mean of all sample points in the cluster as the new centroid position; The similarity of sample points within a cluster is evaluated by the sum of the distances from all boundary points to the centroid. The smaller the sum of distances, the higher the similarity of sample points within the cluster and the smaller the difference. Presets is the i-th point on the K-th cluster, is the centroid of the Kth cluster, Represents the distance from the sample point to the centroid, and the corresponding distance measurement expression is: （7） In the formula, m is the number of clusters, and K is the number of cluster categories; All points in a cluster are divided according to the Euclidean distance To the centroid The sum of squared distances The expression is: （8） Where n is the number of data points in the Kth cluster; The KMeans algorithm is based on the principle of minimum square error and is used to minimize the sum of the squares of the distances from all points to the centroid in each cluster. The corresponding square error It can be expressed as: （9） In the formula, is the ith cluster, is the centroid of the Kth cluster, is the i-th data point in the cluster, d represents the Euclidean distance, Represents all points in the cluster The mean of : （10） In the formula, is the number of points in the K clusters. By calculating the sum of the squared distances from all points in each cluster to the centroid, the KMeans algorithm iterates continuously and updates the position of the centroid to minimize the total squared error of the entire data set.

2. The robot unknown environment exploration method based on the adaptive KMeans algorithm according to claim 1 is characterized in that: The KMeans algorithm is used to perform clustering processing on the boundary points, including: Obtain cluster center points of boundary points, evaluate the cluster center points on the cost map and allocate target points, wherein priority is set for boundary points according to preset evaluation criteria, and information entropy of each boundary point is calculated to evaluate its exploration value, wherein the calculation criteria include selecting appropriate navigation target points according to the calculated information entropy and arranging the robot to explore the location area; Determine the navigation target point based on information entropy. Information entropy is the expected value of the amount of information to describe uncertainty. : （11） In the formula, s is the sample space, is the probability of i occurring; Select the navigation target point through the profit formula of the boundary point: （12） In the formula, is a given frontier point, R represents the revenue of the frontier point, I represents the information increment of the frontier point, is the current location of the robot, is the parameter given by the user, N is the navigation cost of the robot to navigate to the frontier point, is the hysteresis gain used to determine the given leading edge point Is it within the robot's current position? Certain radius In addition, if The value of is equal to 1; otherwise, The value is equal to ,in, Greater than 1; by setting If it is greater than 1, the robot is more inclined to explore the frontier points near it. The corresponding expression is: （13） In the formula, The radius is based on the current robot.

3. The robot unknown environment exploration method based on the adaptive KMeans algorithm according to claim 1 is characterized in that: Guiding the robot to reach the target point based on the TEB path planning algorithm also includes: The optimal target point is determined according to the frontier point, and the key target point is used as the navigation target point in the local path planning of the TEB algorithm. The corresponding robot posture expression is: （14） In the formula, , and is the position and posture of the robot in the map coordinate system, is the state space in the time dimension and the control parameter dimension, indicates that the positions in the coordinate system are all positive real numbers, Indicates all possible status conditions. represents the Cartesian product; The position and posture of the robot in the map coordinate system are taken as the configuration, and the configuration in space can be sorted as follows: （15） There is a time interval between the two configurations, which is defined as , represents the time required for the two configurations of the robot, and the expression of the sequence recording the time is: （16） The configuration and time series are combined as: （17） The optimal path point, that is, the optimal Q, is obtained through weighted multi-objective optimization. The corresponding expression is: （18） （19） In the formula, B is a subset of the arrangement sorting, which is used to evaluate and optimize to obtain the optimal solution; For the best result; is the objective function of the various constraints considered, and is the component of each objective function The weighted sum of is the number of objective functions.

4. The robot unknown environment exploration method based on the adaptive KMeans algorithm according to claim 3 is characterized in that: The first constraint objective function of the TEB algorithm is to follow the path and avoid obstacles. The obstacle avoidance constraint includes following the known global planning path and avoiding obstacles. The configuration sequence is consistent with the global path point sequence or obstacles. The closest distance is , follow the path target to configure the maximum allowed distance from the global path As a constraint, the obstacle avoidance target is configured to have the minimum distance allowed from the obstacle. as a constraint; Constraining functions based on paths and barrier constraint functions With penalty function , the corresponding expression is: （20） In the formula, represents a small displacement next to the limit value, S represents scaling, and n represents the polynomial order.

5. The robot unknown environment exploration method based on the adaptive KMeans algorithm according to claim 4 is characterized in that: The second constraint objective function of the TEB algorithm is the velocity and acceleration constraint. The average linear velocity and angular velocity of the robot can be calculated by the adjacent , and time interval The calculation results are: （21） （22） In the formula, and is the distance between two points, is the rotation angle between two points; The calculation expressions of the robot's average linear acceleration and angular velocity are: （23） Line speed constraints Available penalty functions It is expressed as: （24） The upper and lower bounds of the constraints are determined by combining the upper and lower bounds of the wheel speed and acceleration obtained for the mobile robot, where: Indicates the line speed, Indicates the maximum line speed, represents a small displacement next to the limit value, S represents scaling, and n represents the polynomial order; The motion state of the robot chassis is defined by three key variables: linear movement along the x-axis and y-axis and rotation. The rotation of the axis, Refers to the rotation around the z-axis; decompose the robot chassis motion into three independent variables with the robot's four wheels as the geometric center: represents the x-axis translation, represents the x-axis translation, express The rotation speed of the shaft, the axis speed of the wheel is obtained as: （25） In the formula, is the vector from the geometric center to the wheel axis, is the velocity vector of the wheel axis, The wheel axis is vertical The velocity vector in the direction of the wheel axis is calculated as follows: （26） （27） The speed of the roller can be calculated based on the speed of the wheel axis, which can be decomposed into the speed along the roller direction. and the speed perpendicular to the roller direction , where the vertical direction can be ignored, and the corresponding expression is: （28） in, is the unit vector along the roller direction, so the wheel velocity v is calculated: （29）。 6. The robot unknown environment exploration method based on the adaptive KMeans algorithm according to claim 4 is characterized in that: The third constraint objective function of the TEB algorithm is the kinematic constraint. The robot moves in a smooth trajectory composed of several arc segments. With movement direction , Angle The angle that should correspond to the end configuration Equal, if is the absolute posture of the robot at the i-th arc segment relative to the world coordinate system, then: （30） Among them, the displacement vector in the motion direction is expressed as: （31） In the formula, and Respectively represent the displacement of the coordinates of the two points on the path, and the corresponding objective function : （32） In the formula, and Respectively represent the coordinates of two points on the path. Formula (32) represents the direction of the two points on the path by adding the two vectors; multiplying by the displacement vector , , represents the distance and direction between two points on the path; is the square norm of the vector, which represents the cost function of the path.

7. The robot unknown environment exploration method based on the adaptive KMeans algorithm according to claim 4 is characterized in that: The fourth constraint objective function of the TEB algorithm is the fastest path constraint, which is expressed as minimizing the quadratic time interval sequence: （33） When performing TEB path planning, the Gmapping algorithm is used simultaneously to create a map of the unknown environment. The algorithm constructs a grid map by dividing the two-dimensional space into multiple independent grid cells. Each grid cell is associated with two probability values: represents the probability that the grid cell is a traversable space, represents the probability of being occupied by an obstacle; The sum of the two probability values ​​is 1, and the state of the grid cell can be determined by the ratio of the two probability values. To determine, a higher ratio indicates that the grid cell may be an obstacle; When a specific point in the map receives a new measurement value z, the state needs to be updated accordingly; it is assumed that before the new measurement value arrives, the state of the specific point is known and it is updated to the new estimated state : （34） In the formula, represents the log probability of state s, represents the probability that state s is 1 under given condition z; It represents the probability that state s is 0 under given condition z; According to the Bayesian formula, we can get: （35） And take the logarithm of both sides of formula (35): （36） In the initial state without any measurement value, the initial state of a point is 0, and the ratio is calculated in formula (34) as the model of the measurement value; In fact, there are two models of measurement values: （37） In the formula, both measurements follow a fixed model, so that each time a new measurement is obtained, the state of the point on the map can be updated through simple arithmetic addition and subtraction operations; When constructing a grid map, the robot converts the real-world position into grid coordinates on the map. The length of each grid unit in the map is represented by r. Indicates the resolution of the map; for a two-dimensional grid map, the position of any point can be used to coordinate The coordinates represent the horizontal and vertical distances relative to the origin of the map, where each grid position coordinate is determined by express: （38） In the formula, x and y represent the coordinates in the real world, i and j are the coordinates in the grid map, and r is the length of one grid cell. represents the resolution, then and ; The default robot posture is , the location of obstacles in the map construction process can be obtained : （39） （40） Where d is the measured distance, is the angle between the laser line and the robot's posture angle. After obtaining the two coordinates, the grid positions of the two points are calculated. and , use the Bresenham algorithm to calculate the set of non-obstacle grid points and update the grid map; The coordinates of the i-th pixel point on the preset straight line , the slope of the straight line is 1 and , the possible position of the next pixel on the line is or ,exist The y value of the point on the line is , the distance between the point on the straight line and the pixel point and pixels The distances are and : （41） （42） In the formula, b is the slope of the straight line equation; and The distance difference is: （43） The distance difference can be used to select the next pixel. When the distance difference is positive, , indicating that the theoretical point on the straight line The pixel is closer, the next pixel is taken ; When the distance difference is negative, , indicating that the theoretical point on the straight line is The pixel is closer, then the next pixel is ; When the distance difference is zero, take as the next pixel.

Images