A navigation control method for lane inspection robots

Abstract

The present invention provides a navigation control method for a lane inspection robot, and relates to the technical field of lane robot navigation. The method comprises: obtaining point cloud data collected in real time by a two-dimensional laser radar, generating a target key point cluster; extracting the corresponding environmental shape features, matching them with the lane shape features corresponding to the preset lane type, and determining the current lane type; based on the current lane type and the target key point cluster, establishing a local environment map model of the lane where the lane inspection robot is located, and estimating the posture information of the lane inspection robot according to the key points in the local environment map model; obtaining the coordinates of the task target point, and constructing a real-time local force field based on the local environment map model, the coordinates of the task target point and the posture information; and controlling the real-time motion trajectory of the lane inspection robot to the coordinates of the task target point based on the real-time local force field. The present invention improves the efficiency and safety of autonomous navigation of the lane inspection robot.

Description

Technical Field

[0001] The present invention relates to the technical field of lane robot navigation, and in particular to a navigation control method for a lane inspection robot. Background Art

[0002] Coal mining methods are generally divided into open-pit mining and underground mining. In the process of underground coal mining, due to the differences in coal seams and geological conditions, the environmental conditions of the tunnels are extremely complex, and they are mainly threatened by disasters such as coal and gas outbursts, mine water inrush, and rock bursts. Tunnel safety inspection is an important measure to ensure production safety, and the research and development of tunnel inspection robots has become an important trend in the current development of the coal industry. Reliable navigation technology is the basis for the safe driving of trackless inspection robots. After years of development, autonomous navigation technology has gradually matured and has been widely used in ground mobile robots, but there are still many problems to be solved when applied underground.

[0003] Traditional navigation path planning algorithms and trajectory tracking technologies are highly dependent on the accuracy of global maps, and have poor flexibility and adaptability in different real-time environments. Robots are prone to deviate from preset routes or collide with obstacles, and have high demands on computing resources and storage space. Therefore, the autonomous navigation algorithms of inspection robots in related technologies are difficult to achieve good results in practical applications. Summary of the invention

[0004] The technical problems to be solved by the present invention are: the traditional DBSCAN algorithm causes the inability of the lane inspection robot to accurately identify road condition information due to some incorrect merging and splitting during the clustering process of point cloud data, and the use of the same fitting method for large quantities of data of different complexity leads to excessive demand for computing resources and storage space.

[0005] In order to solve the above problems, the present invention provides a navigation control method for a lane inspection robot, and the technical solution adopted is as follows:

[0006] Obtain point cloud data collected by the two-dimensional laser radar in real time, and generate target key point clusters based on the point cloud data;

[0007] Extract the environment shape features corresponding to the target key point cluster, match the environment shape features with the lane shape features corresponding to the preset lane type, and determine the current lane type faced by the lane inspection robot;

[0008] Based on the current lane type and target key point cluster, a local environment map model of the lane where the lane inspection robot is located is established, and the position information of the lane inspection robot is estimated according to the key points in the local environment map model;

[0009] Obtain the coordinates of the task target point of the lane inspection robot, and build a real-time local force field based on the local environment map model, the coordinates of the task target point and the position information;

[0010] The real-time motion trajectory of the tunnel inspection robot to the coordinates of the mission target point is controlled based on the real-time local force field.

[0011] Compared with the prior art, the present invention has the following beneficial effects:

[0012] The present invention acquires point cloud data collected by a two-dimensional laser radar in real time, and then constructs a local environment map model based on the acquired point cloud data to control the real-time motion trajectory of the tunnel inspection robot. Compared with the high reliance of traditional navigation on the global map, it is easier to avoid deviation from the preset route or collision with obstacles in a timely manner, thereby improving the adaptability of the tunnel inspection robot to emergencies and the safety of the tunnel inspection robot traveling underground.

[0013] The present invention adopts different strategies based on the lane type to establish a local environment map model of the lane where the lane inspection robot is located, so that the constructed local environment map model can be more in line with the real-time terrain characteristics, and improve the accuracy of the environment map model relied on by the lane inspection robot during its driving process;

[0014] The present invention adopts a simple model construction method when facing simple terrain features, which can save the computing resources and storage space of the tunnel inspection robot; the present invention controls the real-time motion trajectory of the tunnel inspection robot to the coordinates of the task target point through a real-time local force field. First, the position of the tunnel inspection robot can be adjusted in real time, so that the robot can flexibly avoid obstacles and dangerous areas, reduce the path planning calculation time and select the optimal path to the target point. At the same time, the control of the real-time local force field can also handle complex working environments including unstructured tunnels, obstacles of different shapes and sizes, etc., so that the stable driving of the tunnel inspection robot is more robust, and the planning of the optimal path can reduce energy consumption and extend the working time of the tunnel inspection robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 The present invention is a flowchart of a navigation control method for a lane inspection robot according to an embodiment of the present invention.

[0016] Figure 2 It is a schematic diagram of a lane inspection robot equipped with a two-dimensional laser radar according to an embodiment of the present invention.

[0017] Figure 3 It is a flow chart of an algorithm for clustering point cloud data to generate initial key point clusters according to an embodiment of the present invention.

[0018] Figure 4 It is a flow chart of an algorithm for merging initial key point clusters based on the similarity between the initial key point clusters to generate a target key point cluster according to an embodiment of the present invention.

[0019] Figure 5 It is a schematic diagram comparing the effects of a clustering algorithm and a similarity calculation method on point cloud data after processing according to an embodiment of the present invention.

[0020] Figure 6 It is a flow chart of an algorithm for establishing a first local model by partial fitting of a simple shape according to an embodiment of the present invention.

[0021] Figure 7 It is a schematic diagram showing the effect of the fitting algorithm on the target key point cluster after processing according to an embodiment of the present invention.

[0022] Figure 8 It is a schematic diagram of extracting information of corner points and center points of intersections according to an embodiment of the present invention.

[0023] Fig. 9 It is a force analysis diagram of a lane inspection robot in a local force field when passing through an intersection, shown in one embodiment of the present invention.

[0024] Fig.10 The figure is a schematic diagram showing the influence of the local force field on the real-time motion trajectory of the lane inspection robot according to an embodiment of the present invention.

[0025] Fig.11 Schematic diagram of the posture of a lane inspection robot at a T-shaped intersection according to an embodiment of the present invention.

[0026] Fig.12 Schematic diagram of the posture of a lane inspection robot at a crossroads according to an embodiment of the present invention.

[0027] Fig.13 Schematic diagram of the posture of a lane inspection robot at a right-angle intersection according to an embodiment of the present invention.

[0028] Fig.14 The figure is a schematic diagram of the motion trajectory of a tunnel inspection robot in a mine, shown in one embodiment of the present invention.

[0029] Fig.15 It is an overall flow chart of controlling a lane inspection robot based on acquired point cloud data, shown in one embodiment of the present invention. DETAILED DESCRIPTION

[0030] The specific implementation of the present invention is further described in detail below in conjunction with the drawings and examples.

[0031] like Figure 1As shown, the present invention discloses a navigation control method for a lane inspection robot, comprising the following steps:

[0032] Step S101, obtaining point cloud data collected in real time by a two-dimensional laser radar, and generating a target key point cluster according to the point cloud data;

[0033] like Figure 2 As shown, the lane inspection robot includes a camera 201, a two-dimensional laser radar 202, a differential drive wheel 203 and a universal support wheel 204. During the movement of the lane inspection robot:

[0034] The camera 201 is used to collect image information of the environment in which the lane inspection robot is located. It transmits real-time image data to the robot system through a visual sensor to help the lane inspection robot perform tasks such as target recognition, route planning, and obstacle avoidance.

[0035] The two-dimensional laser radar 202 is used to measure the distance and shape information of the surrounding environment of the lane inspection robot according to the reflection signal of the emitted laser beam.

[0036] When the lane inspection robot is in a lighted environment, the camera 201 and the two-dimensional laser radar 202 cooperate to acquire the surrounding environment information; when it is in a dark environment, the two-dimensional laser radar 202 acquires the surrounding environment information.

[0037] The differential drive wheel 203 is used to realize the turning and forward and backward movements of the lane inspection robot; the universal support wheel 204 is used to provide support force for the lane inspection robot in different directions to ensure the smooth operation of the lane inspection robot in the lane.

[0038] In the present invention, point cloud data refers to a collection of discrete three-dimensional space points obtained by a two-dimensional laser radar. When the laser beam emitted by the two-dimensional laser radar intersects with an object, a reflection point is generated, and the two-dimensional laser radar records the three-dimensional coordinates and other properties of the reflection point; by continuously emitting and receiving laser beams, the two-dimensional laser radar obtains a series of discrete points, which constitute point cloud data; the density and accuracy of point cloud data depend on the performance and sampling parameters of the two-dimensional laser radar. High-density point cloud data can provide more detailed environmental information, but it will also increase the difficulty of data processing and storage. This embodiment does not specifically limit the performance and sampling parameters of the two-dimensional laser radar.

[0039] In the present invention, the introduction of two-dimensional laser radar is more real-time than the traditional reliance on global maps; and the two-dimensional laser radar has high precision, which can improve the accuracy of the acquired target object detail information; at the same time, the working principle of the laser radar makes the environment less interfere with the data collection work, and improves the anti-interference ability of the tunnel inspection robot during work; and the point cloud data collected by the two-dimensional laser radar can be fused with the data collected by the camera 101, further improving the accuracy and robustness of target detection and positioning.

[0040] In one embodiment of the present invention, in step S101, generating a target key point cluster according to the point cloud data specifically includes:

[0041] Performing clustering processing on the point cloud data to generate initial key point clusters;

[0042] The initial key point clusters are merged based on the similarity between the initial key point clusters to generate a target key point cluster.

[0043] In one embodiment of the present invention, when the environment is harsh, the data extraction effect is poor, or there is noise interference, such as Figure 3 As shown, clustering the point cloud data to generate an initial key point cluster specifically includes:

[0044] S31, randomly selecting an unvisited point in the point cloud data as an implementation point;

[0045] S32, obtaining a first distance between the implementation point and the lane inspection robot, and weighting the first distance to generate a weighted radius coefficient for the implementation point; the weighted radius coefficient calculation formula is as follows: (1)

[0046] Among them, dist p is the first distance of the implementation point, laser range is the scanning distance of the two-dimensional laser radar, the constant C is the initial value of the weighted radius coefficient ε; the first distance is the original distance data between the implementation point and the lane inspection robot.

[0047] S33, obtaining a preset neighborhood radius threshold, and constructing a neighborhood with the implementation point as the center;

[0048] S34, constructing the following exponential decay function (2) based on the weighted radius coefficient and the first distance of the implementation point: (2)

[0049] Among them, ε is the weighted radius coefficient;

[0050] S35, calculating a second distance from the implementation point to each of the remaining data points in the point cloud data, and determining a weighted distance from the implementation point to each of the remaining data points based on the weighted radius coefficient and the second distance;

[0051] S36, comparing the weighted distance from the implementation point to each of the remaining data points with the neighborhood radius threshold, and determining the number of data points in the neighborhood whose weighted distance is less than or equal to the neighborhood radius threshold;

[0052] S37, calculating the neighborhood density of the implementation point according to the number of data points in the neighborhood whose weighted distance is less than or equal to the neighborhood radius threshold; the neighborhood density calculation formula is as follows: (3)

[0053] Where num is the number of data points in the neighborhood whose weighted distance is less than or equal to the neighborhood radius threshold, λ(dist p ) is an exponential decay function, Σλ(dist p ) is the sum of the exponential decay coefficients of all data points in the neighborhood whose weighted distance is less than or equal to the neighborhood radius threshold;

[0054] S38, obtaining a preset density threshold, and judging whether the implementation point is a core point based on the relative size of the neighborhood density of the implementation point and the density threshold;

[0055] If the neighborhood density of the implementation point is less than the density threshold, then traverse the data points in the neighborhood of the implementation point to determine whether the neighborhood density of each data point in the neighborhood is greater than or equal to the density threshold:

[0056] If the neighborhood density of at least one data point in the neighborhood is greater than or equal to the density threshold, the implementation point is added to the initial cluster and marked as a visited point, and the neighbor nodes of the implementation point are recorded through the breadth-first algorithm, and the neighbor nodes are added to the initial key point cluster and marked as visited points;

[0057] If there is no data point in the neighborhood whose neighborhood density is greater than or equal to the density threshold, the implementation point is deleted from the point cloud data;

[0058] S39, repeating steps S31 to S38 until all data points in the point cloud data are traversed, and outputting an initial key point cluster about the point cloud data.

[0059] In the above steps, formula (1) uses the radius coefficient function to weight the distance, which can prevent data points with a long distance from being misjudged as noise points, thereby improving the accuracy of clustering.

[0060] In the above steps, improving the exponential decay function coefficient λ in the area around each core point can reduce the contribution density of data points far away from the core point and optimize the neighborhood density calculation of each core point. Involving the radius coefficient ε as a weight term in the calculation of the exponential function can make the value of the exponential function smoother and give a certain weight to data points far away to prevent the λ value from being too different.

[0061] In the above steps, the breadth-first algorithm is a graph traversal algorithm, which is a well-known technology and will not be described here.

[0062] Steps S31-S39 are the improved clustering algorithm IN-DBSCAN (Improved Neighborhood Density-Based Spatial Clustering of Applications with Noise). Compared with the traditional DBSCAN algorithm that uses a fixed radius to determine the core object, IN-DBSCAN can better reflect the density distribution of data points, thereby providing a more accurate density estimation; further, since the IN-DBSCAN algorithm can more accurately calculate density and adaptively select parameters, it can better discover the clustering structure in the data, and can obtain higher clustering performance when processing data with uneven density distribution, and more accurately identify the boundaries and noise points of the cluster; in addition, the INDBSCAN algorithm calculates density through an exponential decay function, which can better distinguish between noise points and outliers, and has strong robustness to noise and outliers.

[0063] In one embodiment of the present invention, Figure 4 As shown, merging the initial key point clusters based on the similarity between the initial key point clusters to generate a target key point cluster specifically includes:

[0064] S41, calculating the maximum density point and the minimum density point in all initial key point clusters;

[0065] S42, traversing all combinations of two initial key point clusters in all initial key point clusters, and selecting fitting points belonging to the same straight line feature in the two initial key point clusters in the current combination, and forming a first set and a second set respectively in the direction from the point with the largest density to the point with the smallest density in the current two initial key point clusters;

[0066] S43, calculating the dot product of the first set and the second set, the calculation formula is as follows: (4)

[0067] Among them, y i 、z irepresents the normalized result of the i-th vector in the first set and the second set respectively, C represents the dot product result, and n represents the maximum value of the number of vectors in the first set and the number of vectors in the second set;

[0068] S44, based on the dot product result, calculating the similarity between the first set and the second set, the calculation formula is as follows: (5)

[0069] in, , Respectively represent the sum of the modulus lengths of all vectors in the first set and the second set;

[0070] S45, obtaining a preset similarity threshold, if the similarity between the first set and the second set is greater than or equal to the similarity threshold, merging the current two initial key point clusters into a new initial key point cluster, and taking the new initial key point cluster as an initial key point, and returning to step S41;

[0071] If the similarity between the first set and the second set is less than the similarity threshold, mark the combination and return to step S42 to traverse the combination of the remaining two initial key point clusters in all initial key point clusters;

[0072] S46, based on the result of merging all the initial key point clusters, output all the target key point clusters.

[0073] After each merging of two initial key point clusters, all the initial key point clusters after the merging will be traversed again until all the initial key point clusters are traversed, thus ensuring the accuracy of the merging result.

[0074] Figure 5 An example diagram of merging initial key point clusters is shown. In Figure a, there are initial key point clusters 601, 602, 603, 604, 605, and 606, respectively. After steps S41-S46, the initial key point clusters 604 and 605 in Figure a are merged into the same cluster 608 in Figure b, and the initial key point clusters 602 and 603 in Figure a are merged into the same cluster 607 in Figure b, and the clusters 601, 606, 607, and 608 in Figure b are used as the merged target key point clusters.

[0075] The above steps S41-S46 are a cosine similarity calculation method. The present invention adopts the cosine similarity calculation method to merge clusters, which has the following advantages:

[0076] First, cosine similarity is calculated through the angle between vectors. Compared with other similarity calculation methods, it is more suitable for high-dimensional data such as point cloud data, which speeds up the computational efficiency when merging clusters. Secondly, cosine similarity only focuses on the direction of the vector, without considering the specific value of the vector. When the cosine similarity calculation method is used, even if the data has a large offset or scaling, the similarity calculation results described can still maintain a certain stability. Finally, the calculation of cosine similarity only involves the calculation of the inner product of two vectors and the length of the vector. The calculation process is simple and efficient, especially when processing large-scale point cloud data collected in real time by two-dimensional lidar. It is more advantageous and efficient.

[0077] Step S102, extracting the environment shape features corresponding to the target key point cluster, matching the environment shape features with the lane shape features corresponding to the preset lane type, and determining the current lane type faced by the lane inspection robot;

[0078] In one embodiment of the present invention, the environmental shape feature corresponding to the target key point cluster is at least one of the features such as size, density, shape, compactness or separation corresponding to the target key point cluster.

[0079] The tunnel type refers to the channel form of the tunnel space where the tunnel inspection robot is located. In the present invention, when matching the environmental shape features with the tunnel shape features corresponding to the preset tunnel type, they can be matched by level and item, or multiple features can be matched simultaneously, and the present invention does not make any special limitations.

[0080] By matching the environmental shape features extracted from the target key point cluster with the lane shape features, the type of the current lane can be accurately determined, which improves the judgment accuracy of the lane inspection robot and further improves the safety of the lane inspection robot driving in complex terrain. After determining the lane type in which the lane inspection robot is located, different inspection strategies and path planning can be adopted for different lane types, thereby improving inspection efficiency.

[0081] Step S103, based on the current lane type and the target key point cluster, establish a local environment map model of the lane where the lane inspection robot is located, and estimate the position and posture information of the lane inspection robot according to the key points in the local environment map model;

[0082] Among them, the local environment map model of the alley refers to a feature map fitted by feature points collected by two-dimensional laser radar, which is used to characterize the local environment path characteristics. The local environment map model may include the geometric shape of the alley, and may also include information such as the alley size and alley channel layout.

[0083] In one embodiment of the present invention, a local environment map model of the lane where the lane inspection robot is located is established by a fitting algorithm, specifically:

[0084] When the lane type is an intersection, based on the target key point cluster and the shape of the target key point cluster, a multi-fitting algorithm is used to establish a local environment map model of the intersection where the lane inspection robot is located by region;

[0085] When the lane type is a road section, based on the target key point cluster and the preset center angle, a fitting algorithm is used to establish a fan-shaped local environment map model under the center angles on both sides of the lane inspection robot.

[0086] For the two different types of road conditions, intersections and road sections, different methods are used when establishing the local environment map model. Resources can be reasonably allocated for road conditions of different difficulty levels, thereby reducing the amount of calculation and storage requirements and improving modeling efficiency.

[0087] In one embodiment of the present invention, when the lane type is an intersection, the steps of establishing a local environment map model of the intersection where the lane inspection robot is located are as follows:

[0088] A long strip-shaped region of interest is set for each target key point cluster, and a target key point cluster in which data points in the long strip-shaped region of interest are linearly arranged is divided into a simple shape part, and the remaining target key point clusters are divided into a complex shape part;

[0089] For the simple shape part, the first local model is established by fitting; for the complex shape part, the second local model is established by fitting;

[0090] The first local model and the second local model are combined to establish a local environment map model of the intersection where the alley inspection robot is located in a robot coordinate system with the alley inspection robot as the origin and the forward direction of the alley inspection robot as the positive direction of the X-axis.

[0091] In the above steps, the long strip region of interest must be centered at the center point of each target key point cluster, and the long strip region of interest must include the target key point cluster; the long strip region of interest can be adjusted according to the size of the target key point cluster.

[0092] In one embodiment of the present invention, Figure 6 As shown, for a simple shape part, the steps of fitting and establishing the first local model are as follows:

[0093] S71, sorting the density of the point cloud data constituting the simple shape part, determining the point with the largest density, and randomly selecting a sample point;

[0094] S72, constructing a sample model based on the maximum density point and the sample points;

[0095] S73, traversing other data points in the simple shape part except the sample point and the point with the highest density, and calculating the fitting errors between other data points and the sample model;

[0096] S74, obtaining a preset fitting threshold, marking the data points whose fitting errors are less than or equal to the fitting threshold as inliers, and adding the inliers to the sample model;

[0097] S75, obtain the preset minimum inlier threshold, calculate the number of inliers, and determine whether the number of inliers is less than the minimum inlier threshold. If the number of inliers is less than the minimum inlier threshold, re-execute steps S71 to S75; if the number of inliers is greater than or equal to the minimum inlier threshold, use the sample model as the first local model.

[0098] The fitting error refers to the difference or deviation between other points and the sample model. The smaller the fitting error, the closer the fitting result is to the actual data, and the higher the accuracy of the model. The fitting error can be measured by different indicators, such as mean square error, mean absolute error, or root mean square error. This embodiment does not specifically limit the selection of the fitting error measurement indicator.

[0099] The present invention can reduce the influence of outliers on model fitting and improve the robustness of the model by screening out inliers.

[0100] The I-RANSAC optimization algorithm used in the above steps optimizes the selection method of sample points compared to the traditional RANSAC algorithm. Through the information obtained by the clustering algorithm, the points with the highest density are preferentially selected as sample points. The density of each core point is calculated according to the clustering algorithm, and the parameters and topological structure of the optimal model are dynamically adjusted, which reduces the influence of outliers on model fitting and improves the robustness of the model fitting algorithm. Secondly, by randomly sampling and selecting a data subset for model fitting, the number of inliers is calculated according to the fitting results, and the proportion of data-intensive points in the model is increased, which can increase the number of inliers, narrow the search space, and reduce the number of iterations, thereby improving the efficiency of the algorithm.

[0101] In one embodiment of the present invention, for a part with a complex shape, Hough transform fitting is used to establish a second local model.

[0102] Hough transform is a well-known technology in the field of image data processing. In the present invention, Hough transform is used to process two-dimensional point cloud data. The process is the same as that of image data and will not be repeated here.

[0103] like Figure 7As shown, Figure a is the first local model constructed by the I-RANSAC algorithm for fitting the simple shape part, and Figure b is the second local model constructed by the Hough transform for the complex shape part. In the figure, the hexagons are the model corner points and the straight lines are the fitted models.

[0104] This embodiment uses different model fitting methods for target key point clusters according to their different shapes, which can improve the navigation and planning efficiency of the robot during the inspection process, allowing the robot to complete the inspection task more quickly and accurately.

[0105] When the lane type is a road section, the 30° area on both sides of the robot is extracted for fitting modeling. The distance between the robot and the lane wall and the heading angle are estimated by fitting the parameters of the straight line model. That is, in the robot coordinate system with the lane inspection robot as the origin and the forward direction of the lane inspection robot as the positive direction of the X-axis, the distance between the fitting straight line and the origin of the coordinate system is used as the estimated distance between the robot and the lane wall. The heading angle of the robot can be obtained by the slope of the straight line model, and the establishment of the fan-shaped local environment map model under the center angle of the lane inspection robot on both sides is completed.

[0106] In other embodiments of the present invention, regions of 45° or 60° on both sides of the robot may also be extracted for fitting modeling.

[0107] By establishing a local environment map model of the alley inspection robot based on the current alley type and target key point cluster, the accuracy of the robot's positioning can be improved. The real-time construction of the model for the real-time alley can ensure the alley inspection robot's real-time perception of the environment and maintain the accuracy and practicality of the local environment map model. At the same time, by obtaining the position and posture information of the alley inspection robot, it can be precisely controlled and path planned to ensure that the alley inspection robot can accurately perform inspection tasks and reduce the collision rate and error accumulation.

[0108] The key points in the local environment map model refer to points that are used to characterize the environment characteristics in the lane environment. In one embodiment of the present invention, the coordinates of the key points in the local environment map model in the robot coordinate system are extracted, and the coordinates of the center point of the intersection are calculated. The position and posture of the lane inspection robot are estimated in reverse through the coordinates of the key points and the coordinates of the center point of the intersection.

[0109] Step S104, obtaining the task target point coordinates of the lane inspection robot, and constructing a real-time local force field based on the local environment map model, the task target point coordinates and the position and posture information;

[0110] In the present invention, the task target point coordinates of the lane inspection robot refer to the coordinates of the staged moving target points planned at each movement of the lane inspection robot to prevent it from touching the lane wall or obstacles when passing through a certain lane type. The task target point coordinates can be at the center of the lane section, or at the position of the road section close to the wall, the center of the intersection, or the position close to the corner of the intersection, etc.

[0111] In one embodiment of the present invention, the real-time position of the tunnel inspection robot is taken as the origin, the target point provided by the planner is set as the attraction point, and the point cloud data of the tunnel wall and obstacles are set as the repulsion point to construct a local force field that can be updated in real time.

[0112] By obtaining the coordinates of the mission target point, the alley inspection robot can accurately navigate to the specified location according to the pre-set path or task list, thereby improving the inspection efficiency and navigation accuracy. Secondly, the establishment of the local environment map model enables the alley inspection robot to have a clearer perception of the surrounding environment, so that it can better avoid obstacles and avoid touching the alley walls, thereby improving the safety of the alley inspection robot during driving. Finally, the real-time local force field can be dynamically adjusted according to the robot's current position and the surrounding environmental changes, so that the alley inspection robot can respond quickly to re-plan the driving route when encountering emergencies, thereby improving the adaptability and driving safety of the alley inspection robot.

[0113] Step S105, controlling the real-time motion trajectory of the lane inspection robot to the coordinates of the task target point based on the real-time local force field.

[0114] In one embodiment of the present invention, step S105 is specifically as follows: determining the gravitational function and the repulsive function in the real-time local force field, adjusting the weights of each force in the two functions to generate the sum of the real-time minimum forces, and then determining the real-time heading angle of the alley inspection robot, and comprehensively controlling the real-time motion trajectory of the alley inspection robot by combining the real-time heading angle of the alley inspection robot and the weights of each force.

[0115] In one embodiment of the present invention, the real-time position of the tunnel inspection robot is taken as the origin, the task target point provided by the planner is set as the attraction point, and the point cloud data of the tunnel wall and obstacles are set as the repulsion point. The movement direction and speed of the tunnel inspection robot are determined by calculating the position of the environment surrounding the tunnel inspection robot. The minimization of the sum of the potential energy on the motion trajectory of the tunnel inspection robot is used as the evaluation index, and the weights of each force in the potential energy function are adjusted in real time to achieve autonomous navigation and control of the robot.

[0116] The gravity function describes the gravitational force between the robot and the target point. For example, the following function: Among them, F grepresents the gravitational force on the lane inspection robot, k is a constant coefficient, x target and x robot They represent the mission target point and the position of the lane inspection robot respectively.

[0117] The repulsion function describes the repulsion between the lane inspection robot and the obstacle. For example, the following function: Among them, F r represents the repulsive force on the lane inspection robot, k is a constant coefficient, r represents the distance between the lane inspection robot and the obstacle, d max is a maximum threshold used to determine the strength of the repulsive force of obstacles on the robot.

[0118] In practical applications, as the environment in which the lane inspection robot is located is different, the gravity function needs to be adjusted and optimized according to specific needs to adapt to different working environments and task requirements. Therefore, this embodiment does not specifically limit the specific forms of the gravity function and the repulsion function.

[0119] The motion trajectory and heading angle of the alley inspection robot are controlled by dynamically adjusting the sum of the minimum potential energy to achieve the purpose of driving into and out of the intersection. First, when the alley inspection robot completes the alley inspection task, the motion trajectory and heading angle are flexibly adjusted according to the actual situation, which can ensure that the robot can accurately cover all positions in the alley and avoid missed inspections or repeated inspections. Secondly, by controlling the real-time motion trajectory through the sum of the minimum potential energy, it can select the best motion trajectory and heading angle, reduce unnecessary movement and energy consumption, and improve inspection efficiency. In addition, whether facing a crossroads, a T-shaped intersection or a right-angle intersection, it can make corresponding adjustments according to the actual situation, thereby improving the adaptability of the alley inspection robot. Finally, when encountering obstacles or emergencies, the alley inspection robot can avoid obstacles or take corresponding actions through dynamic adjustments, thereby improving the robustness of the alley inspection robot's smooth driving and further improving its safety during driving.

[0120] Furthermore, step S105 can use the local force field to make the alley inspection robot move in the direction of decreasing potential energy, while using the PID controller to adjust the parameters such as the position, speed and acceleration of the alley inspection robot in real time, and use the particle swarm optimization algorithm to optimize the parameters of the PID controller so that the alley inspection robot can move according to the set trajectory.

[0121] In one embodiment of the present invention, an adjustment coefficient is added to optimize the potential field distribution. When the robot is in a single-lane lane, the lane wall always gives the robot a repulsive force. According to the distance value obtained, the corresponding potential field attenuation coefficient is set. When the robot is close to the wall, a larger attenuation coefficient should be set to prevent the robot from hitting the wall or getting too close to the wall and scratching it. At the same time, a certain area within a certain distance between the robot and the lane wall is set as a dead zone to prevent the robot from getting too close to the wall and interfering with the robot's movement and positioning accuracy. When specifying the inspection area, we need to set appropriate potential field parameters according to the characteristics of the area and the robot's motion trajectory, and set the wall's potential field attenuation coefficient to 0, that is, it is not affected by the wall, to ensure that the robot can perform inspection tasks safely and efficiently.

[0122] In one embodiment of the present invention, the following closed-loop control is used to enable the lane inspection robot to pass through the intersection smoothly:

[0123] First, the traffic plan is determined based on the information provided by the planner. When entering an intersection, the center point of the intersection gives the robot gravity, and the robot is driven by gravity to a certain range of the center point of the intersection. Then, the heading angle is adjusted according to the planner's traffic plan to face the lateral center point of the section to be entered. When leaving the intersection, the center point of the section to be entered gives the robot gravity, and the center point of the intersection instead provides repulsion until it enters the range of the lane section.

[0124] The lane inspection robot is allowed to pass through intersections through closed-loop control. On the one hand, closed-loop control can monitor and adjust the robot's motion state in real time, allowing it to more accurately control the robot's behavior at intersections. Through sensors and feedback control systems, the robot's position, speed, direction and other parameters can be detected in real time, and corrected according to actual conditions, thereby improving control accuracy. On the other hand, when faced with emergencies, closed-loop control can be used to respond and adjust to environmental changes in real time, thereby improving the robot's driving stability and robustness, and ensuring its driving safety in complex situations.

[0125] The following is an example diagram provided by the present invention:

[0126] When the lane inspection robot is in an intersection, it first needs to Figure 8The extraction of the corner point and center point information of the intersection shown in the figure, where the square area is the position of the lane inspection robot, and the hexagonal mark is the key point of the intersection and the center point of the intersection. The key point of the intersection in Figure c is located at the four corner points that make up the crossroads, and the lane inspection robot is at the center point of the intersection; the key points of the intersection in Figure d include the two corner points of the T-shaped intersection and the intersection of the perpendicular midline of the straight line connecting the two corner points and the lane wall, and the center point of the intersection is located at the centroid of the isosceles triangle surrounded by the above three points; in Figure e, the two corner points of the intersection are located at the foot of the perpendicular of the corner of the intersection, and the center point of the intersection is located at the midpoint of the straight line connecting the two corner points.

[0127] Take the example of a traffic intersection. Fig. 9 As shown, Figure f is a schematic diagram of the sum of the forces on the lane inspection robot when entering the intersection, and Figure g is a schematic diagram of the sum of the forces on the lane inspection robot when leaving the intersection. During the passage, the robot first extracts the coordinates of the corner points of the intersection for positioning to determine the relative coordinates of the center point of the intersection and the relative position of the robot. During the robot's driving process, it is affected by the potential field generated by the environment. When entering the intersection, the robot is still within the potential field influence range of the obstacle (Obstacle), so it is affected by the repulsive force F_rep generated by the obstacle; since the robot is in the specified driving area, the repulsive forces F_rep1 and F_rep2 provided by the lane walls on both sides are 0; the center point of the intersection gives the robot gravity to guide the robot to enter a certain range of the intersection center. When leaving the intersection, the robot needs to overcome the repulsive force F_rep2 generated by the obstacle (Obstacle) and the repulsive force F_rep1 generated by the center point of the intersection, and is guided by the lateral center point (Goal) of the section to be entered to leave the intersection. Specifically, the robot's motion trajectory and heading angle are affected by the force exerted by the environmental potential field. On this basis, the robot's speed is dynamically adjusted to achieve the purpose of entering and leaving the intersection.

[0128] Fig.10 The influence of the potential field on the real-time trajectory of the alley inspection robot is demonstrated. Figures h, i, and j are schematic diagrams of the influence of the potential field on the real-time trajectory of the alley inspection robot when the alley inspection robot is at a crossroad, a T-shaped intersection, and a right-angle intersection, respectively. The trajectory is linear, the square area is the starting position of the alley inspection robot, the triangular area is the ending position of the alley inspection robot, the circle area is the obstacle, and the hexagon is the key point of the intersection.

[0129] In one embodiment of the present invention, the Gazebo simulation environment is used to simulate the lane. Fig.11 , 121 and 13 respectively show the visualization interface of the posture and point cloud data of the lane inspection robot in the lane environment of T-junction, crossroad and right-angle intersection simulated by the Gazebo simulation environment in the robot operating system (ROS), where two postures are detected for each type of intersection. The discrete points in the figure represent the point cloud data collected by the sensor, and the straight line is the geometric model constructed for the surrounding environment using the algorithm. In addition, the points intersecting the model are the key point information of the intersection calculated by the robot fitting, and the point located at the center of the road section is the relative coordinate of the center point of the intersection. Among them, when entering the intersection from the positions k, m, and o in the figure and arriving at the positions l, n, and p in the figure, a new environment model is reconstructed based on the new data collected by the two-dimensional laser radar, and the viewing direction is adaptively adjusted.

[0130] like Fig.14 The figure shows a trajectory diagram of the robot's local positioning mapped to the global positioning, where the square area is the starting position of the alley inspection robot, the point in the center of the alley is the running trajectory point of the alley inspection robot, the arrow indicates its heading angle at that point, and the hexagon is the key point of the intersection, i.e., the center point of the intersection.

[0131] like Fig.15 The figure shows an overall flow chart of a control method for a lane inspection robot according to an embodiment of the present invention. In the data collection and preprocessing stage, the point cloud data is preliminarily processed by correcting the data collected by the two-dimensional laser radar, converting the coordinate system, and performing denoising filtering. Next, the clustering algorithm and the model fitting algorithm are used in combination with the information provided by the planner to realize the clustering fitting of the point cloud data and the pose estimation of the lane inspection robot. Then, the local force field is set and the controller makes motion decisions based on the minimum force resultant. The particle swarm optimization algorithm (PSO) is used to optimize the parameters of the PID controller to generate the best real-time motion decision, and finally the underlying motor is controlled to move.

[0132] The above implementation modes are only used to illustrate the present invention, but not to limit the present invention. Ordinary technicians in the relevant technical field can make various changes and modifications without departing from the spirit and scope of the present invention. Therefore, all equivalent technical solutions also belong to the scope of the present invention. The patent protection scope of the present invention should be defined by the claims.

Claims

1. A navigation control method for a lane inspection robot, which is executed by a lane inspection robot equipped with a two-dimensional laser radar, characterized in that: The method comprises: Acquire point cloud data collected in real time by a two-dimensional laser radar, and generate a target key point cluster based on the point cloud data; Extracting the environment shape features corresponding to the target key point cluster, matching the environment shape features with the lane shape features corresponding to the preset lane type, and determining the current lane type faced by the lane inspection robot; Based on the current lane type and the target key point cluster, a local environment map model of the lane where the lane inspection robot is located is established, and the position and posture information of the lane inspection robot is estimated according to the key points in the local environment map model; Acquire the task target point coordinates of the lane inspection robot, and construct a real-time local force field based on the local environment map model, the task target point coordinates and the posture information; The real-time motion trajectory of the lane inspection robot to the coordinates of the task target point is controlled based on the real-time local force field.

2. The navigation control method for a lane inspection robot according to claim 1, characterized in that: Generating a target key point cluster according to the point cloud data includes: Performing clustering processing on the point cloud data to generate initial key point clusters; The initial key point clusters are merged based on the similarity between the initial key point clusters to generate a target key point cluster.

3. The navigation control method for the lane inspection robot according to claim 2 is characterized in that: The step of clustering the point cloud data to generate an initial key point cluster includes: S31, randomly selecting an unvisited point in the point cloud data as an implementation point; S32, obtaining a first distance between the implementation point and the lane inspection robot, and weighting the first distance to generate a weighted radius coefficient for the implementation point; the weighted radius coefficient calculation formula is as follows: （1） Among them, dist p is the first distance of the implementation point, laser range is the scanning distance of the two-dimensional laser radar, and the constant C is the initial value of the weighted radius coefficient ε; S33, obtaining a preset neighborhood radius threshold, and constructing a neighborhood with the implementation point as the center; S34, constructing the following exponential decay function (2) based on the weighted radius coefficient and the first distance of the implementation point: （2） Among them, ε is the weighted radius coefficient; S35, calculating a second distance from the implementation point to each of the remaining data points in the point cloud data, and determining a weighted distance from the implementation point to each of the remaining data points based on the weighted radius coefficient and the second distance; S36, comparing the weighted distance from the implementation point to each of the remaining data points with the neighborhood radius threshold, and determining the number of data points in the neighborhood whose weighted distance is less than or equal to the neighborhood radius threshold; S37, calculating the neighborhood density of the implementation point according to the number of data points in the neighborhood whose weighted distance is less than or equal to the neighborhood radius threshold; the neighborhood density calculation formula is as follows: (3) Where num is the number of data points in the neighborhood whose weighted distance is less than or equal to the neighborhood radius threshold, λ(dist p ) is an exponential decay function, Σλ(dist p ) is the sum of the exponential decay coefficients of all data points in the neighborhood whose weighted distance is less than or equal to the neighborhood radius threshold; S38, obtaining a preset density threshold, and judging whether the implementation point is a core point based on the relative size of the neighborhood density of the implementation point and the density threshold; If the neighborhood density of the implementation point is less than the density threshold, then traverse the data points in the neighborhood of the implementation point to determine whether the neighborhood density of each data point in the neighborhood is greater than or equal to the density threshold: If the neighborhood density of at least one data point in the neighborhood is greater than or equal to the density threshold, the implementation point is added to the initial cluster and marked as a visited point, and the neighbor nodes of the implementation point are recorded through the breadth-first algorithm, and the neighbor nodes are added to the initial key point cluster and marked as visited points; If there is no data point in the neighborhood whose neighborhood density is greater than or equal to the density threshold, the implementation point is deleted from the point cloud data; S39, repeating steps S31 to S38 until all data points in the point cloud data are traversed, and outputting an initial key point cluster about the point cloud data.

4. The navigation control method for a lane inspection robot according to claim 2, characterized in that: The merging of the initial key point clusters based on the similarity between the initial key point clusters to generate a target key point cluster includes: S41, calculating the maximum density point and the minimum density point in all initial key point clusters; S42, traversing all combinations of two initial key point clusters in all initial key point clusters, and selecting fitting points belonging to the same straight line feature in the two initial key point clusters in the current combination, and forming a first set and a second set respectively in the direction from the point with the largest density to the point with the smallest density in the current two initial key point clusters; S43, calculating the dot product of the first set and the second set, and the calculation formula is as follows: （4） Among them, y i 、z i represents the normalized result of the i-th vector in the first set and the second set respectively, C represents the dot product result, and n represents the maximum value of the number of vectors in the first set and the number of vectors in the second set; S44, based on the dot product result, calculating the similarity between the first set and the second set, the calculation formula is as follows: (5) in, , represent the sum of all vector moduli in the first set and the second set respectively; S45, obtaining a preset similarity threshold, if the similarity between the first set and the second set is greater than or equal to the similarity threshold, merging the current two initial key point clusters into a new initial key point cluster, and taking the new initial key point cluster as an initial key point, and returning to step S41; if the similarity between the first set and the second set is less than the similarity threshold, marking the combination mode and returning to step S42, traversing the combination modes of the remaining two initial key point clusters in all initial key point clusters; S46, based on the result of merging all the initial key point clusters, output all the target key point clusters.

5. The navigation control method for a lane inspection robot according to claim 1, characterized in that: The lane types include intersections and road sections. Based on the lane types and the target key point clusters, a local environment map model of the lane where the lane inspection robot is located is established using a fitting algorithm, including: When the lane type is an intersection, a local environment map model of the intersection where the lane inspection robot is located is established by region using a multi-fitting algorithm based on the target key point cluster and the shape of the target key point cluster; When the lane type is a road section, based on the target key point cluster and the preset center angle, a fitting algorithm is used to establish a fan-shaped local environment map model at the center angle on both sides of the lane inspection robot.

6. The navigation control method for the lane inspection robot according to claim 5, characterized in that: The method of establishing a local environment map model of the intersection where the lane inspection robot is located by region using a multi-fitting algorithm based on the target key point cluster and according to the complexity of the target key point cluster includes: A long strip-shaped region of interest is set for each target key point cluster, and the target key point clusters with linear arrangement trend of data points in the region of interest are divided into simple shape parts, and the remaining target key point clusters are divided into complex shape parts; Establishing a first local model for the simple shape partial fitting; For the complex shape part, a Hough transform fitting algorithm is used to fit and establish a second local model; The first local model and the second local model are combined to establish a local environment map model of the intersection where the alley inspection robot is located in a robot coordinate system with the alley inspection robot as the origin and the forward direction of the alley inspection robot as the positive direction of the X-axis.

7. The navigation control method for the lane inspection robot according to claim 6, characterized in that: The step of partially fitting and establishing a first local model for the simple shape comprises: S71, sorting the density of the point cloud data constituting the simple shape part, determining the point with the maximum density, and randomly selecting a sample point; S72, constructing a sample model based on the maximum density point and the sample points; S73, traversing other data points in the simple shape part except the sample point and the point with the highest density, and calculating the fitting errors between other data points and the sample model; S74, obtaining a preset fitting threshold, marking the data points whose fitting errors are less than or equal to the fitting threshold as inliers, and adding the inliers to the sample model; S75, obtain a preset minimum inlier threshold, calculate the number of inliers, and determine whether the number of inliers is less than the minimum inlier threshold. If the number of inliers is less than the minimum inlier threshold, re-execute steps S71 to S75; if the number of inliers is greater than or equal to the minimum inlier threshold, use the sample model as the first local model.

8. The navigation control method for a lane inspection robot according to claim 1, characterized in that: The real-time control of the real-time motion trajectory of the lane inspection robot based on the real-time local force field includes: Determine the gravitational function and the repulsive function in the real-time local force field based on the real-time local force field; Generating a real-time minimum sum of forces by adjusting the weights of the forces in the attraction function and the repulsion function; Determining the real-time heading angle of the lane inspection robot based on the sum of the real-time minimum forces; The real-time motion trajectory of the lane inspection robot is controlled by controlling the real-time heading angle of the lane inspection robot and the weights of the forces.

Images