Vision-intelligent-based path planning method and system for inspection robot

Abstract

This invention relates to the field of automatic control technology and discloses a path planning method and system for inspection robots based on visual intelligence. The method includes: collecting raw obstacle data through multi-sensor fusion; estimating obstacle states and predicting future trajectories using particle filtering; extracting passable areas based on the predicted trajectories; generating candidate paths using an ant colony optimization algorithm; selecting the optimal path through safety screening and priority ranking; dynamically adjusting and smoothing the path in conjunction with environmental changes; and finally completing the inspection task through PID control and visual detection. This invention, through the synergistic application of multiple technologies, effectively solves the problems of high collision risk and easy planning failure caused by uncertain obstacle trajectories in dynamic environments, significantly improving the autonomy, safety, and task completion efficiency of inspection robots in complex dynamic scenarios.

Description

Technical Field

[0001] This invention relates to the field of automatic control technology, and in particular to a path planning method and system for inspection robots based on visual intelligence. Background Technology

[0002] Currently, intelligent inspection technology is being applied more and more widely. Inspection robots, with their advantages of high efficiency and safety, are gradually replacing manual labor in high-risk, repetitive inspection tasks, becoming core equipment for ensuring the stable operation of facilities. Visual intelligence, as a key technology for robots to perceive their environment, directly affects the accuracy and reliability of path planning in areas such as dynamic obstacle recognition and scene feature extraction.

[0003] In existing technologies, inspection robot path planning is often based on preset maps or static obstacle models, generating fixed paths through traditional algorithms. While this can meet the needs of simple scenarios, it does not fully integrate the real-time perception capabilities of visual intelligence. When facing moving obstacles, this type of solution relies solely on data from a single sensor for immediate obstacle avoidance, lacking a mechanism to predict the obstacle's trajectory. This results in frequent path adjustments, stiff obstacle avoidance actions, and difficulty adapting to changes in complex and dynamic environments.

[0004] In existing technologies, most path planning methods do not effectively integrate the dynamic perception and trajectory prediction capabilities of visual intelligence, and cannot accurately capture the speed changes and motion patterns of moving obstacles. When faced with sudden obstacles, they are prone to planning delays or collision risks, resulting in low efficiency and insufficient reliability of inspection tasks. Summary of the Invention

[0005] This invention provides a path planning method and system for inspection robots based on visual intelligence, in order to solve the problems of low efficiency and insufficient reliability of inspection tasks in existing technologies.

[0006] Firstly, to address the aforementioned technical problems, this invention provides a path planning method for inspection robots based on visual intelligence, comprising:

[0007] The raw data of moving obstacles in a dynamic environment is acquired, and the raw data of moving obstacles is processed by particle filtering to obtain the current position and velocity information of the obstacles.

[0008] Historical trajectory data is obtained based on the current position and speed information of the obstacle and the data is fused. Abnormal speed changes are identified and particle weights are updated. The future trajectory of the obstacle is determined by extrapolation calculation.

[0009] Based on the future trajectory of the obstacles, a set of potential collision areas is determined, the environmental map is divided, and a set of passable areas is extracted.

[0010] The ant colony optimization algorithm is used to simulate path search for the set of passable areas, generating a set of candidate path sequences.

[0011] Based on the candidate path sequence set, calculate the safety distance index matrix and the path length index vector, filter the effective path subset and sort them in descending order, and determine the path priority ranking.

[0012] The highest priority path in the priority ranking is selected, and the path sequence is dynamically adjusted according to changes in the surrounding environment. After smoothing, the optimized final path is obtained.

[0013] Based on the optimized final path, a motion control command sequence is generated, the actual trajectory deviation value is calculated, and the motor speed is corrected in real time. The completion status of the inspection task is determined by the feature point matching rate of the inspection target area.

[0014] Secondly, the present invention provides a path planning system for inspection robots based on visual intelligence, comprising:

[0015] The data acquisition module is used to acquire raw data of moving obstacles in a dynamic environment, and to process the raw data of moving obstacles through particle filtering to obtain the current position and speed information of the obstacles.

[0016] The trajectory prediction module is used to acquire historical trajectory data based on the current position and speed information of the obstacle and perform data fusion, determine abnormal speed changes and update particle weights, and determine the future trajectory of the obstacle through extrapolation calculation.

[0017] The region extraction module is used to determine a set of potential collision regions based on the future trajectory of the obstacle, divide the environment map, and extract a set of passable regions.

[0018] The path generation module is used to simulate path search on the set of passable areas using an ant colony optimization algorithm to generate a set of candidate path sequences.

[0019] The filtering and sorting module is used to calculate the safety distance index matrix and the path length index vector based on the candidate path sequence set, filter the effective path subset and sort it in descending order, and determine the path priority ranking.

[0020] The optimization and adjustment module is used to select the highest priority path in the priority sorting, dynamically obtain the adjustment path sequence according to changes in the surrounding environment, and obtain the optimized final path after smoothing.

[0021] The task execution module is used to generate a motion control command sequence based on the optimized final path, calculate the actual trajectory deviation value and correct the motor speed in real time, determine whether the task is completed by the feature point matching rate of the inspection target area, and obtain the inspection task termination status.

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

[0023] (1) This invention obtains the original data of moving obstacles in a dynamic environment by multi-sensor fusion, combines the data with particle filtering and integrates historical trajectory data, so as to accurately obtain the current position and speed information of the obstacle and make high-precision prediction of the future trajectory, effectively reducing the collision risk in a dynamic environment.

[0024] (2) Based on the future trajectory of obstacles, the present invention determines the set of potential collision areas, divides the environmental map and extracts the set of passable areas, generates a set of candidate path sequences through ant colony optimization algorithm, and combines the safety distance index matrix and path length index vector for screening and sorting to ensure that the planned path is both safe and efficient.

[0025] (3) After selecting the highest priority path, the present invention dynamically adjusts the path sequence by integrating real-time environmental changes, and obtains the optimized final path by smoothing through Bézier curves. At the same time, the actual trajectory deviation value is calculated by feedback loop and the motor speed is corrected in real time. Relying on visual intelligent detection of the feature point matching rate of the inspection target area, the robustness and completion efficiency of the inspection task are significantly improved. Attached Figure Description

[0026] Figure 1 This is a schematic diagram of the path planning method for inspection robots based on visual intelligence provided in the first embodiment of the present invention;

[0027] Figure 2 This is a schematic diagram of the path planning system for an inspection robot based on visual intelligence provided in the second embodiment of the present invention. Detailed Implementation

[0028] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0029] Reference Figure 1 The first embodiment of the present invention provides a path planning method for an inspection robot based on visual intelligence, comprising the following steps:

[0030] S11 is used to acquire raw data of moving obstacles in a dynamic environment, and process the raw data of moving obstacles through particle filtering to obtain the current position and speed information of the obstacles;

[0031] S12 is used to obtain historical trajectory data based on the current position and speed information of the obstacle and perform data fusion, determine abnormal speed changes and update particle weights, and determine the future trajectory of the obstacle through extrapolation calculation;

[0032] S13, used to determine a set of potential collision areas based on the future trajectory of the obstacle, divide the environment map and extract a set of passable areas;

[0033] S14 is used to simulate path search on the set of passable areas using an ant colony optimization algorithm to generate a set of candidate path sequences.

[0034] S15 is used to calculate the safety distance index matrix and the path length index vector based on the candidate path sequence set, filter the effective path subset and sort it in descending order, and determine the path priority ranking.

[0035] S16 is used to select the highest priority path in the priority sorting, and dynamically obtain the adjusted path sequence according to changes in the surrounding environment, and obtain the optimized final path after smoothing.

[0036] S17 is used to generate a motion control command sequence based on the optimized final path, calculate the actual trajectory deviation value and correct the motor speed in real time, determine whether the task is completed by the feature point matching rate of the inspection target area, and obtain the inspection task termination status.

[0037] In step S11, it is necessary to acquire the raw data of moving obstacles in the dynamic environment, group and calculate the observation positions, establish the correspondence between obstacles in consecutive frames, and process the data through particle filtering to determine the current position and velocity information of the obstacles, including:

[0038] Raw data of moving obstacles in a dynamic environment is obtained through multi-sensor fusion, and the raw data of moving obstacles includes distance data and angle data;

[0039] The raw data of the moving obstacles is grouped using the DBSCAN clustering algorithm to obtain a first set of points corresponding to multiple independent obstacles;

[0040] The centroid position, circumscribed rectangle boundary, and orientation of each obstacle are calculated based on the first point set to obtain the observation position of the obstacle in the current frame;

[0041] Obtain the predicted position distance of the tracked obstacle in the previous frame. If the difference between the predicted position distance and the observed position distance of the obstacle in the current frame is less than a preset distance threshold, then determine the association matching and establish the correspondence between obstacles in consecutive frames.

[0042] The correspondence between obstacles in the continuous frames is maintained by particle filtering to maintain the particle set. The current position and velocity information of the obstacles are determined by updating the particle weights and resampling.

[0043] It should be noted that the preset distance threshold value is derived from historical measured data across all different scenarios. Taking into account the moving speed of common obstacles (0-15m / s), sensor sampling frequency (10Hz), and positioning error (±0.3m), when the threshold is set to 1.5 meters, the accuracy of inter-frame association matching of obstacles can reach over 95%, with a missed match rate of less than 3% and a false match rate of less than 2%, reducing computational overhead while ensuring matching accuracy. In complex scenarios with dense obstacles and high sensor accuracy, the threshold can be reduced to improve matching accuracy; in simple scenarios with sparse obstacles and limited sensor accuracy, the threshold can be increased to reduce the risk of missed matches.

[0044] Particle filtering is a state estimation method based on Monte Carlo simulation. Its core is to maintain a set of random particles to characterize the probability distribution of obstacle position and velocity. It is suitable for handling nonlinear and non-Gaussian systems in dynamic environments and can effectively cope with the uncertainties caused by sensor noise and obstacle occlusion. It is a key technology for achieving accurate obstacle state estimation.

[0045] The particle ensemble is the core of particle filtering. Each particle contains information about the position and velocity state of the obstacle. The number of particles needs to be balanced between accuracy and efficiency. In this embodiment, it is set to 1000, which can ensure the estimation accuracy without excessively consuming computing resources. The particle weight is used to measure the degree of matching between each particle and the actual observation. The larger the weight, the closer the state represented by the particle is to the true state of the obstacle. It is the core basis for particle updating and resampling.

[0046] In this step, multi-sensor fusion data acquisition is first performed, using LiDAR and millimeter-wave radar to work together. The acquisition time of the two sensors is unified by timestamp alignment (synchronization accuracy ±1ms). Then, the polar coordinate data of the millimeter-wave radar is converted into the Cartesian coordinate system of the LiDAR. Finally, the data is fused to form the original data of moving obstacles in a unified coordinate system, which has both high precision and anti-interference capabilities.

[0047] Furthermore, the DBSCAN clustering algorithm is executed for grouping. This algorithm does not require pre-setting the number of clusters and is naturally robust to noisy points. The neighborhood radius is then set. The value is determined based on the minimum size of common obstacles to ensure that point cloud data of the same obstacle are clustered into one class; the minimum number of points, MinPts=5, is set according to the number of effective points of obstacles in a single frame scan by the LiDAR to avoid misclassifying isolated noise points as obstacles. The DBSCAN clustering algorithm divides densely connected points into the same group by judging the density connectivity of data points, thereby distinguishing independent obstacles. Each group corresponds to one obstacle, resulting in the first set of points for multiple independent obstacles.

[0048] Furthermore, it is necessary to calculate the observation position of the obstacle in the current frame. The centroid position is obtained by calculating the arithmetic mean of the coordinates of all points in the first point set, which can intuitively reflect the center position of the obstacle. The boundary of the circumscribed rectangle is obtained by fitting the minimum circumscribed rectangle. By rotating the coordinate system, the rectangle with the smallest area that can contain all points is found, and the coordinates of the lower left and upper right vertices of the rectangle are output to clearly define the spatial range of the obstacle. The orientation of the obstacle is determined by extracting the major axis direction of the circumscribed rectangle. The major axis direction vector is roughly consistent with the direction of movement of the obstacle, providing a reference for subsequent trajectory prediction. Finally, the observation positions are integrated to form the observation position.

[0049] Then, the correspondence between obstacles in consecutive frames is established. The position and velocity information of obstacles in the previous frame are predicted using a Kalman filter algorithm to obtain the predicted position of the obstacles in the current frame. The prediction is based on a constant-velocity motion model. First, a state vector is defined, containing the x-axis position, y-axis position, x-axis velocity, and y-axis velocity of the obstacles in the previous frame. The time interval is set to 0.1 seconds, consistent with the sensor's 10Hz sampling frequency. The state transition matrix is ​​constructed based on the constant-velocity assumption and is used to extrapolate the predicted state from the previous frame to the current frame. Specifically, the position equals the previous position plus the product of the velocity and the time interval, while the velocity remains consistent with the previous frame. Simultaneously, a process noise covariance matrix is ​​introduced, with the position noise variance set to 0.01 m² and the velocity noise variance set to 0.001 m² / s² to simulate uncertainties during motion. Substituting the state estimate and covariance matrix from the previous frame's particle filter output into the state transition formula, we obtain the predicted state (including predicted position and velocity) and the updated covariance matrix for the current frame. The predicted position is the core data used for association and matching with the observation position in the current frame. We calculate the Euclidean distance between the observation position in the current frame and the predicted position. If the distance is less than a preset distance threshold of 1.5 meters, it is determined to be the same obstacle, and an association relationship between consecutive frames is established. If the distance is greater than the threshold, it is determined to be a newly appeared obstacle, and its tracking state is initialized.

[0050] Finally, particle filtering is used to determine the current position and velocity information of the obstacle. 1000 particles are initialized, with particle positions sampled near the observed centroid using a Gaussian distribution (mean = observed centroid coordinates, variance = 0.1 m²), and velocities sampled using a uniform distribution (covering the velocity range of common obstacles 0-15 m / s). A constant-velocity motion model is used to update the particle state, while process noise (variance = 0.01 m² / s²) is added to simulate the uncertainty of obstacle motion. Particle weights are updated based on the Mahalanobis distance between the observed centroid and the particle position; Mahalanobis distance effectively considers data correlation, and the weights are normalized after updating. A roulette wheel resampling method is used to retain particles with larger weights and remove particles with smaller weights, ensuring that the particle set focuses on high-probability regions. Finally, the weighted average of the particles is used as the current position and velocity information of the obstacle, with the weights being the normalized weights of the particles, achieving high-precision state estimation.

[0051] For example, in a scenario along a main urban road, LiDAR and millimeter-wave radar collaboratively collect data, which is then fused to obtain over 300 valid points ahead; the DBSCAN clustering algorithm is then applied (…). The points (MinPts=5) are grouped into 4 groups, corresponding to a car, a truck, and two pedestrians respectively, resulting in the first point set for each obstacle. The centroid position is calculated based on the first point set corresponding to the car, and the arithmetic mean of the point set coordinates is taken, indicating the centroid is located at (8.5m, -1.2m) in the sensor coordinate system. A minimum bounding rectangle is fitted to obtain a bounding rectangle with a length of 6.8m and a width of 2.3m. The major axis direction angle is extracted to be 12 degrees, determining the vehicle's orientation parallel to the road. The previous frame... The predicted position of the car was (8.2m, -1.1m). The Euclidean distance between the current observed centroid and the predicted position was 0.42m, which was less than the preset distance threshold of 1.5m. Therefore, the correlation was determined and a correspondence between consecutive frames was established. Particle filtering was used to maintain 1000 particles. After updating the motion model, calculating the weights, and resampling, the current position of the car was obtained as (8.48m, -1.19m), and the speed information was (14.8m / s, 0.3m / s), achieving high-precision state estimation.

[0052] In step S12, historical trajectory data needs to be obtained and fused based on the current position and velocity information of the obstacle. Abnormal velocity changes are identified and particle weights are updated. The future trajectory of the obstacle is determined through extrapolation calculation, including:

[0053] Historical trajectory data is obtained based on the current position and speed information of the obstacle, and the historical trajectory data is fused to obtain a continuous trajectory sequence.

[0054] Calculate the velocity vector difference between adjacent frames in the continuous trajectory sequence. If the velocity vector difference exceeds a preset vector threshold, it is marked as an abnormal velocity change.

[0055] If the speed change is abnormal, the particle weight set is updated by particle filtering, and the continuous trajectory sequence is extrapolated based on the updated particle weight set to determine the future trajectory of the obstacle.

[0056] It should be noted that the historical trajectory data is a historical record of the inspection robot's movement status of obstacles, including core information such as timestamps, position coordinates, and velocity vectors. The data source is the obstacle status information output in each frame in step S11, and it is stored in the robot's local storage module in CSV format. Each record contains a timestamp accurate to milliseconds, position coordinates with meter-level accuracy, and a velocity vector with meter / second-level accuracy.

[0057] The preset vector threshold is derived from statistics on the motion characteristics of common obstacles. The speed change rate for pedestrians walking normally typically does not exceed 1 m / s, for non-motorized vehicles it does not exceed 1.5 m / s, and for motorized vehicles driving normally it does not exceed 2 m / s. Only under special circumstances such as braking or rapid acceleration will the speed change rate exceed 2 m / s. Therefore, a threshold of 2 m / s can effectively distinguish between normal and abnormal speed changes. This threshold can be dynamically adjusted according to different application scenarios.

[0058] Abnormal speed changes are identified using Boolean values: "True" indicates a drastic speed change, while "False" indicates a normal speed change. This identifier is a key signal that triggers particle weight updates. When an abnormal speed change is detected, the particle weight set needs to be updated accordingly to adjust the particle distribution, making the particles more focused on states that match the actual speed changes and improving the accuracy of state estimation. The future trajectory of an obstacle is a predicted future movement path based on the current and historical states, with a prediction time window set to 2 seconds. This time window takes into account the inspection robot's movement speed (1-5 m / s) and path planning cycle (0.5 seconds) to ensure that the robot has sufficient time to adjust its path based on the prediction results and avoid collision risks. The future trajectory is presented as a set of predicted position points, including the position coordinates every 0.2 seconds within the next 2 seconds.

[0059] In this step, the historical trajectory data of the obstacle over the past 5 seconds (50 frames in total, corresponding to a 10Hz sampling frequency) is first retrieved from the local storage module. Breakpoints are then processed through trajectory stitching. The trajectory stitching uses linear interpolation. If the time interval between two adjacent frames exceeds 1.5 times the sensor sampling period (i.e., 0.15 seconds), it is determined to be a trajectory breakpoint. Based on the position coordinates and velocity vectors of the frames before and after the breakpoint, the state information at the breakpoint is completed using linear interpolation over time, forming a continuous trajectory sequence.

[0060] Furthermore, the continuous trajectory sequence is traversed, and the velocity vector difference between adjacent frames is calculated frame by frame. The calculated result is compared with a preset vector threshold of 2 m / s. If the velocity vector difference of a certain frame exceeds 2 m / s, the frame is marked as having an abnormal velocity change, and the abnormal velocity change flag is set to "true". If the velocity vector difference of all frames in the continuous trajectory sequence does not exceed the threshold, the abnormal velocity change flag is set to "false".

[0061] Furthermore, when an abnormal velocity change is detected, it indicates a drastic change in the obstacle's motion state, requiring adjustment of the particle filter's observation model to adapt to this change. Specifically, the observation noise weight in the region of abnormal velocity change is increased. The particle weight update formula adds an influence factor of the velocity vector difference to the original Mahalanobis distance, giving higher weights to particles consistent with the actual velocity change trend. After the weight update, normalization is performed to ensure that the sum of all particle weights is 1. If no abnormal velocity change is detected, the original particle weight set is used without additional updates.

[0062] Finally, future trajectory extrapolation is performed. Position and velocity data from the most recent 10 frames of the continuous trajectory sequence are selected as fitting samples, and a quadratic polynomial fitting algorithm is used to construct the obstacle's motion model. The quadratic polynomial can better capture the nonlinear motion trends of obstacles (such as acceleration, deceleration, and turning), and has higher prediction accuracy compared to linear fitting. The fitting coefficients are solved using the least squares method. The core of the least squares method is to minimize the sum of squared errors between the fitted values ​​and the actual observed values, ensuring that the fitted model reflects the actual motion law of the obstacle to the greatest extent. Based on the solved fitting model, the position coordinates are predicted every 0.2 seconds within the next 2 seconds. These predicted coordinates are arranged in chronological order to form the obstacle's future trajectory (predicted position point set).

[0063] For example, in an urban road scenario, the system tracks a car traveling at a stable speed, recording its position and speed information every 0.1 seconds, accumulating 50 historical points over the past 5 seconds. During this period, the car is briefly obscured by roadside trees, resulting in two missing frames of data. After filling the gaps using linear interpolation, a continuous trajectory sequence is formed. When the car suddenly brakes, its speed drops from 10 m / s to 6 m / s, and the calculated speed vector difference is 4 m / s, exceeding the preset vector threshold of 2 m / s. The system marks the abnormal speed change as "true." Based on this flag, the particle filter observation model is adjusted, and the particle weight set is updated so that particles that match the braking motion trend receive higher weights. The system selects the state data from the most recent 10 frames and obtains a motion model through quadratic polynomial fitting. Based on this model, the system predicts the car's position coordinates within the next 2 seconds, obtaining the future trajectory where the car will further decelerate to 3 m / s 20 meters ahead, providing an accurate predictive basis for path planning.

[0064] In step S13, it is necessary to determine a set of potential collision areas based on the future trajectory of the obstacle, divide the environment map, and extract a set of passable areas, including:

[0065] From the future trajectory of the obstacle, obtain a set of all predicted location points, and extend a safe distance for each predicted location point to obtain a set of potential collision areas;

[0066] Based on the set of potential collision regions, an environmental map is obtained and divided into uniform grid cells. It is determined whether each grid cell overlaps with the set of potential collision regions. The overlapping grid cells are marked as occupied grids to form an occupied grid set.

[0067] The occupied grid set is subjected to morphological dilation, and connected components are marked to obtain a set of independent obstacle regions;

[0068] The remaining grids in the environment map, excluding the set of independent obstacle areas, are assessed for connectivity, and connected grids are merged to obtain a set of passable areas.

[0069] It should be noted that the predicted location point set is a core component of the obstacle's future trajectory. It contains the predicted location coordinates every 0.2 seconds within the next 2 seconds. Each coordinate point precisely corresponds to the obstacle's expected position at that moment and is the basic data for constructing the potential collision area.

[0070] Determining the safe distance requires comprehensive consideration of the inspection robot's physical dimensions, movement speed, and braking distance. In dynamic environments, a 2-meter safe distance ensures that even if an obstacle suddenly moves, the robot has sufficient space and time to brake or avoid it. Actual testing has verified that the collision risk at this safe distance is less than 0.1%. The safe distance can be dynamically adjusted according to the specific application scenario. When the robot is large (length > 2 meters) or moves at a high speed (> 5 m / s), the safe distance can be increased to 2.5-3 meters; when the robot is small (length < 1 meter) or moves at a slow speed (< 1 m / s), the safe distance can be reduced to 1.5 meters, improving path planning efficiency while ensuring safety.

[0071] The potential collision zone set is a risk prediction of the future movement range of an obstacle. Each predicted location point is extended by a 2-meter safety distance to form a square area with a side length of 4 meters. This area includes all the spatial range that the obstacle may affect at that moment. The potential collision zones at multiple moments are combined to form a complete risk zone set, providing clear risk identification for subsequent environmental modeling.

[0072] An environmental map is a digital representation of the inspection robot's working environment, including information such as the boundaries of the work area and fixed obstacles (e.g., buildings, equipment, trees). Data sources are twofold: real-time mapping, where the robot scans the work area using sensors before the inspection task begins to build a map in real time; and offline mapping, where the map is pre-scanned and pre-processed using specialized equipment and stored locally on the robot. The map coordinate system is consistent with the sensor coordinate system, and the resolution is set to 0.5 meters per pixel to ensure the map accurately reflects environmental details without excessively consuming storage and computing resources.

[0073] A uniform grid cell is a square grid that divides the environmental map into fixed sizes, with a side length of 1 meter. This size balances planning accuracy and computational efficiency. A 1-meter grid can accurately distinguish between passable areas and obstacle areas, meeting the path planning needs of the inspection robot. At the same time, the number of grids is moderate, so that the computational load will not be too large due to the grid being too fine, nor will the planning accuracy be insufficient due to the grid being too coarse.

[0074] The occupied grid set is a collection of grid cells in the environment map that pose a collision risk. It is determined by whether a grid overlaps with a potential collision area; if they overlap, they are marked as occupied grids, which intuitively reflect the risk areas in the environment. The independent obstacle area set is a further processing result of the occupied grids. Through morphological dilation and connectivity labeling, the scattered occupied grids are integrated into complete obstacle areas, and each independent obstacle area corresponds to a continuous risk range. The passable area set is the remaining area in the environment map after removing obstacle areas. Adjacent passable grids are merged through connectivity determination to form multiple continuous passable areas, providing a clear spatial range for subsequent path search.

[0075] In this step, all predicted location points are first extracted from the future trajectory of the obstacle. Each predicted location point serves as the center of a square area, and a safe distance of 2 meters is extended outwards to form a square potential collision area with a side length of 4 meters. The potential collision areas corresponding to all predicted location points are combined to form a set of potential collision areas.

[0076] Furthermore, an environmental map is acquired and divided into uniform grid cells of 1m x 1m size, with each grid cell uniquely identified by the coordinates of its lower left corner vertex. In one feasible approach, an axis-aligned bounding box (AABB) overlap detection algorithm can be used to determine whether each grid cell overlaps with any region in the potential collision region set. If the boundary of a grid cell intersects with the boundary of a potential collision region, it is considered an overlap, and the grid cell is marked as an occupied grid. After traversing all grid cells, all occupied grids form an occupied grid set.

[0077] Furthermore, morphological dilation is performed on the occupied grid set. First, a 3×3 square structural element is determined (the center and 8 neighboring positions are all effective action points). Each grid cell in the occupied grid set is traversed, and that grid cell and all grid cells within its 3×3 neighborhood are marked as occupied grids. After traversal, the originally scattered occupied grids expand outwards, forming a dilated occupied grid set with an increased range. This compensates for the risks caused by trajectory prediction errors and avoids collisions due to deviations between predicted and actual positions. After the dilation operation, seed filling is used to mark the connected components of the dilated occupied grids. Starting from an unmarked occupied grid, all adjacent occupied grids (in the four directions of up, down, left, and right) are recursively marked, forming a connected component. This process is repeated until all occupied grids are marked. Each connected component corresponds to an independent obstacle region, and all independent obstacle regions form an independent obstacle region set.

[0078] Finally, the set of passable regions is extracted. This involves extracting the remaining grids from the environment map after removing the sets of independent obstacle areas, forming a set of potentially passable grids. The connectivity between adjacent grids (in the four directions: up, down, left, and right) in the potentially passable grid set is determined. If two grids are adjacent and both are potentially passable, they are considered connected. All connected grids are merged into one passable region. After traversing all potentially passable grids, multiple consecutive passable regions are obtained, which are then combined to form a set of passable regions.

[0079] For example, in an urban road scenario, the predicted location of an obstacle 2 seconds from now contains 10 location points. Each point is extended by a 2-meter safety distance to form 10 square potential collision areas of 4 meters × 4 meters, which are combined into a potential collision area set. The environmental map of the urban road is obtained and divided into uniform grid cells of 1 meter × 1 meter size. The AABB overlap detection algorithm is used to mark the grid cells that overlap with the potential collision areas, forming an occupied grid set. The morphological dilation operation of 3 × 3 structuring elements is performed on the occupied grid set to expand the risk area range. Seed filling is used to mark connected components, and two independent obstacle areas are identified, forming an independent obstacle area set. The remaining grid cells in the environmental map after removing the obstacle areas are extracted, and connected grid cells are merged by connectivity judgment, finally obtaining three continuous passable areas, forming a passable area set, which provides a clear spatial range for subsequent path search.

[0080] In step S14, the set of passable areas needs to be simulated using an ant colony optimization algorithm to generate a set of candidate path sequences, including:

[0081] Obtain the starting grid and ending grid from the set of passable areas, generate a path node graph, and initialize the pheromone concentration matrix of the ant colony optimization algorithm to obtain the initial set of ant colony individuals;

[0082] Based on the initial set of individual ant colonies, simulate path movement on the path node graph to obtain a single-round path node sequence set;

[0083] For the single-round path node sequence set, collision detection is used to filter out collision-free path node sequences, and the path heuristic distance is evaluated to obtain a path quality score set;

[0084] The pheromone concentration matrix is ​​updated based on the path quality score set, and after iterating through a preset number of rounds, high-concentration paths are extracted to generate a candidate path sequence set.

[0085] It should be noted that the starting grid is the grid cell where the inspection robot is currently located. It is determined by the fusion positioning of GPS and inertial measurement unit (IMU), with a positioning accuracy better than 0.5 meters, and can be accurately mapped to the corresponding grid on the environmental map. The ending grid is the grid cell corresponding to the target location of the inspection task. It is obtained by mapping according to the preset inspection target coordinates and is the endpoint of the path search.

[0086] The path node graph is the search foundation of the ant colony optimization algorithm. Each grid cell in the set of walkable regions is considered a node. If two grid cells are adjacent (in all four directions: up, down, left, right, and diagonally; diagonally adjacent cells must be unobstructed along the diagonal) and both are walkable, a bidirectional edge is established between the two nodes. The edge weight is set based on the distance, with the weight value consistent with the actual distance: a straight-line adjacent edge has a weight of 1, and a diagonally adjacent edge has a weight of 0. This forms a connected graph structure.

[0087] The pheromone concentration matrix is ​​the core mechanism of the ant colony optimization algorithm. The matrix dimension matches the number of grid cells in the environment map. Each element corresponds to the pheromone concentration of an edge in the path node graph. The pheromone concentration indicates the quality of the path; a higher concentration indicates a better path. The initial value of the pheromone concentration matrix is ​​set to 1.0, with a range limited to 0.1-10.0. Setting a minimum value of 0.1 prevents the algorithm from getting stuck in local optima due to excessively low pheromone concentration, ensuring the algorithm's exploration ability. Setting a maximum value of 10.0 prevents the algorithm from converging too quickly due to excessively high pheromone concentration, thus losing the ability to search for better paths.

[0088] The initial ant colony consists of a set of virtual ants, each representing a potential path search subject. The colony size is set to 30 ants, a number determined based on the best experimental results. Too few ants will limit the search range and make it difficult to find the optimal path; too many ants will increase computational overhead and reduce the real-time performance of the algorithm. 30 ants can achieve a balance between search range and computational efficiency.

[0089] The preset number of iterations needs to be verified through experiments. After the preset number of iterations, the algorithm can converge to a better path solution, and the calculation time is controlled within 0.5 seconds, which meets the real-time planning requirements of the inspection robot. In this embodiment, 50 iterations are used as an example, and can be dynamically adjusted according to the actual application scenario.

[0090] In this step, the path node graph is first constructed and the parameters are initialized. After determining the starting grid and the ending grid, the grid cells of the passable area are used as nodes, and edges are established according to the adjacency relationship to generate the path node graph. The pheromone concentration matrix is ​​initialized, and the pheromone concentration of all edges is set to 1.0. An initial ant colony of 30 virtual ants is generated, and the initial position of all ants is set to the starting grid.

[0091] Furthermore, the movement of ants along their paths is simulated. Each ant starts from its starting grid and selects its next grid node based on a transition probability, which is related to the pheromone concentration of the edge and the heuristic distance. The pheromone heuristic factor... Take 1.5, the heuristic function factor. Take 2.0, The influence weight used to adjust pheromone concentration The influence weight of the heuristic distance is used to adjust the value, which has been experimentally verified to balance the exploratory nature (searching for new paths) and the utilization (selecting known optimal paths) of the algorithm. Different combinations are tested using the controlled variable method. Take 1.0-2.0, Using a range of 1.5-2.5, we statistically analyzed the path convergence speed, the proportion of optimal paths, and the computation time, and found that... A value of 1.5 can prevent pheromone over-dominance from leading to local optima. A value of 2.0 enhances heuristic distance guidance and improves search efficiency. The path planning success rate under this combination exceeds 98%, hence this value was chosen. Each ant moves only from nodes within its passable area until it reaches the endpoint grid or gets stuck in a dead end (i.e., the current node has no adjacent passable nodes). Each ant's movement trajectory is recorded as a path node sequence, and all ant path node sequences form a single-round path node sequence set.

[0092] Furthermore, collision detection is performed on the single-round path node sequence set. All nodes of each path are traversed; if any node occupies a grid, the path is considered a collision path and is removed. Collision-free paths are retained where all nodes are passable grids. For collision-free paths, the sum of their path length and the straight-line distance to the destination is calculated as a path quality score; a smaller score indicates higher quality.

[0093] Pheromone renewal involves two processes: evaporation and deposition. The evaporation process is described by the evaporation coefficient. =0.1 Execute, multiply the pheromone concentration of all edges by (1- This operation can forget the pheromones of poor paths, avoiding the algorithm getting trapped in local optima. The pheromone deposition process only occurs on the edges of collision-free paths. The increment of deposited pheromones is inversely proportional to the path quality score; the higher the path quality score, the larger the increment of deposited pheromones. The pheromone increment equals the pheromone increment constant divided by the path quality score. The pheromone increment constant Q is set to 100 to ensure that the magnitude of the pheromone increment is moderate. This value is based on the measured range of path quality scores and the convergence target of 50 iterations. After multiple sets of experiments, it has been verified that when Q=100, the pheromone increment of high-quality paths and the increment of poor paths form a reasonable gradient, which can quickly guide the ant colony to gather on high-quality paths while retaining the exploration space for other paths, ensuring that the algorithm converges to the optimal solution within 50 rounds. Therefore, Q=100 is determined. If the updated pheromone concentration exceeds the range of 0.1-10.0, it is truncated to the corresponding boundary value.

[0094] Finally, iteratively search and extract candidate paths. Repeat the above process of path movement, collision detection, quality assessment, and pheromone update until the number of iterations reaches 50 rounds. After the iteration, extract continuous edge sequences with pheromone concentrations exceeding 3.5. These sequences correspond to high-quality paths obtained by the algorithm convergence. Select 3-5 paths to form a candidate path sequence set, providing multiple alternative schemes for subsequent path selection and ranking.

[0095] For example, in an autonomous driving scenario on urban roads, the starting grid is the grid at row 10, column 15 in the lower left corner of the map, and the ending grid is the grid at row 80, column 65 (the target parking space); the path node graph contains 800 passable nodes and 5000 valid edges; the pheromone concentration matrix is ​​initialized to 1.0, generating an initial ant colony of 30 ants; in the first iteration, the ants search for paths based on transition probabilities, generating 30 single-round path node sequences; collision detection removes 18 paths containing occupied grids, retaining 12 collision-free paths; the quality scores of these paths are calculated, among which one... A path with a length of 92 meters and a straight-line distance of 65 meters received a quality score of 157, while another longer but smoother path, with a length of 108 meters and a straight-line distance of the same 65 meters, received a quality score of 173. The lower the path quality score, the higher the quality; therefore, the path with a quality score of 157 was selected. Pheromones were evaporated at an evaporation coefficient of 0.1, and pheromones were deposited on the edges of the 12 effective paths, with higher-quality paths receiving more incremental pheromones. After 50 iterations, pheromones significantly clustered on 3 high-quality paths, and paths with pheromone concentrations exceeding 3.5 were extracted to generate a candidate path sequence set.

[0096] In step S15, based on the candidate path sequence set, it is necessary to calculate the safety distance index matrix and the path length index vector, filter the effective path subset and sort it in descending order, and determine the path priority ranking, including:

[0097] Obtain the node coordinate sequence of each path from the candidate path sequence set, calculate the Euclidean distance set with the nearest occupied grid boundary, and form a safety distance index matrix;

[0098] Determine whether there is a distance value in the safety distance index matrix that is less than a preset safety threshold. If so, mark the path corresponding to the distance value as an invalid path to obtain a subset of valid paths.

[0099] Calculate the path length index vector of the effective path subset, and weight and fuse the average value of the path length index vector and the safety distance index matrix to generate a comprehensive priority score vector;

[0100] The effective path subset is sorted in descending order based on the comprehensive priority score vector to determine the path priority ranking.

[0101] It should be noted that the node coordinate sequence is a sequence of coordinates that converts the grid nodes of the path into coordinates in the actual environment. Each grid node corresponds to an actual coordinate (grid center coordinates), which facilitates the calculation of the distance to the obstacle area.

[0102] The safety distance index matrix represents the safety level of each path, consisting of the Euclidean distance between each node on the path and the nearest occupied grid boundary. The larger the distance, the safer the path. The preset safety threshold is 2.0 meters, consistent with the safety distance of potential collision areas, ensuring sufficient safety margin between the path and obstacle areas. This threshold can be adjusted according to the safety requirements of the scenario. For high-safety-requirement scenarios (such as industrial workshops where obstacles are mostly heavy equipment), it can be increased to 2.5 meters; for general scenarios, it can be kept at 2.0 meters; and for low-safety-requirement scenarios (such as open parks), it can be reduced to 1.5 meters.

[0103] The path length index vector is a set of effective path lengths. The length of each path is obtained by summing the Euclidean distances of adjacent nodes, which intuitively reflects the driving efficiency of the path. The comprehensive priority score vector is a comprehensive evaluation result of effective paths, which combines path length and safety distance. The higher the score, the better the overall performance of the path.

[0104] In this step, the sequence of grid nodes for each candidate path is first converted into an actual coordinate sequence (grid center coordinates). Then, each node along each path is traversed, and the Euclidean distance between that node and the nearest occupied grid boundary is calculated. During the calculation, the occupied grids in eight directions around the node are scanned to find the nearest grid boundary. The straight-line distance from the node to this boundary is calculated, and the distances of all nodes are arranged in order to form the safety distance index matrix for that path.

[0105] Furthermore, the safety distance index matrix of each candidate path is traversed. If any distance value in the matrix is ​​less than the preset safety threshold of 2.0 meters, the path is marked as an invalid path and removed from the candidate path sequence set. Paths with a safety distance of not less than 2.0 meters for all nodes are retained to form a subset of valid paths.

[0106] Furthermore, the length of each path in the subset of valid paths is calculated, and the Euclidean distances between adjacent nodes on each path are summed to obtain the total length of each path. The total lengths of all paths form a path length index vector. Simultaneously, the average value of the safety distance index matrix for each path is calculated to reflect the overall safety level of the path. A weighted summation is used to calculate the comprehensive priority score. Specifically, for each valid path, its path length is calculated... and average safe distance Perform min-max normalization on each value, mapping them to the [0,1] interval to obtain the standardized values. and Comprehensive priority score ,in, and These are the path length weighting coefficient and the safety distance weighting coefficient, which can be set as follows, respectively. =0.65 and =0.35, which is based on statistical data from multi-scenario experiments. It achieves the best balance in path length optimization while ensuring a safe distance of no less than 2.0 meters. This value can be adjusted according to specific scenarios; for example, in high-risk environments, the safety weight can be increased to above 0.5. Ultimately, The higher the value, the better the overall path performance.

[0107] Finally, the paths are prioritized and sorted in descending order based on their overall priority scores. The path with the highest overall priority score is ranked first, and so on, thus forming a path priority ranking. The paths ranked higher have both shorter lengths and higher security.

[0108] It is worth noting that the weighting of path length and safety distance is determined based on the requirements of the inspection scenario. In most inspection tasks, both efficiency and safety are important, but path length has a greater impact on inspection efficiency. Therefore, path length is given a higher weight. In this embodiment, the path length weighting coefficient is 0.65 and the safety distance weighting coefficient is 0.35 as an example. If the scenario has extremely high safety requirements (such as inspection of high-risk environments), the safety distance weight can be increased to 0.5-0.6 and the path length weight can be decreased to 0.4-0.5. If the scenario has extremely high efficiency requirements (such as large-scale rapid inspection), the path length weight can be increased to 0.7-0.8 and the safety distance weight can be decreased to 0.2-0.3.

[0109] For example, the ant colony optimization algorithm yields 5 candidate path sequences, which then enter the safety screening and priority ranking stage: the grid nodes of each path are converted into actual coordinate sequences, and the Euclidean distance between each node and the nearest occupied grid boundary is calculated to form a safety distance index matrix; one path, when crossing a narrow section, has a node only 1.6 meters away from a parked vehicle on the roadside, which is less than the preset safety threshold of 2.0 meters, and is marked as an invalid path and eliminated, ultimately retaining 3 valid paths; the lengths of the 3 valid paths are calculated to be 94.8 meters, 101.2 meters, and 97.5 meters, respectively, with average safety distances of 4.1 meters, 5.3 meters, and 4.7 meters, respectively; after standardizing the path lengths and average safety distances, they are weighted and fused using weight coefficients of 0.65 and 0.35, respectively, to calculate comprehensive priority scores of 0.83, 0.78, and 0.81, respectively; the quicksort algorithm is used to sort them in descending order of comprehensive priority scores, and the final path priority ranking is first, third, and second, with the first-ranked path having both the shortest length and a high safety distance.

[0110] In step S16, the highest priority path in the priority ranking needs to be selected, and the path sequence is dynamically adjusted based on changes in the surrounding environment. After smoothing, the optimized final path is obtained, including:

[0111] The highest priority path in the priority ranking is selected as the initial selected path sequence, and the changes in the surrounding environment of the initial selected path sequence are monitored to obtain new obstacle coordinate data.

[0112] Calculate the nearest Euclidean distance between the new obstacle coordinate data and each node of the initially selected path sequence. If there is a distance value less than a preset safety threshold, trigger replanning to generate a new path priority sort and select the highest priority path as the adjusted path sequence. If there is no such path, retain the initially selected path sequence as the adjusted path sequence.

[0113] The adjusted path sequence is fused with the new obstacle coordinate data, and the node coordinates of the adjusted path sequence are smoothed using Bézier curves to obtain the optimized final path.

[0114] It should be noted that the new obstacle coordinate data is acquired through multi-sensor fusion data collection in step S11. The monitoring range is a 5-meter area on both sides of the initially selected path sequence. This range ensures timely detection of newly appearing obstacles around the path, allowing sufficient time for path adjustment. The new obstacle coordinate data is information on previously unidentified obstacles detected through multi-sensor fusion data collection, including the obstacle's center coordinates, occupied area (length, width), and orientation angle. The data format is consistent with the obstacle observation position in step S11, ensuring data consistency and compatibility.

[0115] The preset safety threshold is consistent with the preset safety threshold in step S15, and is set to 2.0 meters to ensure that the safety standards are consistent before and after the path adjustment, avoiding collision risks caused by inconsistent safety thresholds. The adjusted path sequence is the path adjusted according to environmental changes. If the initially selected path has no collision risk, the adjusted path sequence is the same as the initially selected path sequence; if there is a collision risk, a new path priority ranking is generated by replanning, and the new highest priority path is selected as the adjusted path sequence.

[0116] A Bézier curve is a parametric curve. In this embodiment, a third-order Bézier curve is used for path smoothing. The third-order Bézier curve is determined by four control points and has the characteristics of continuous curvature and good smoothness. It can effectively eliminate the broken line transitions between path nodes, making the path more consistent with the robot's motion characteristics, reducing the frequency of motor control adjustments, and reducing energy consumption.

[0117] In this step, the path with the highest overall priority score is first selected directly from the path priority ranking as the initial selected path sequence. This path does not require further screening and is directly used as the robot's initial driving path, avoiding decision-making delays. Then, environmental changes are monitored and new obstacle data is acquired. Environmental data around the initially selected path sequence is continuously collected through multi-sensor fusion. Combined with visual intelligence technology, newly appearing obstacles are perceived, and the center coordinates, occupied area, and orientation angle of the new obstacles are extracted to form new obstacle coordinate data.

[0118] Further, collision risk is assessed and an adjustment path sequence is determined. The nearest Euclidean distance between the new obstacle coordinates and each node of the initially selected path sequence is calculated. Specifically, the new obstacle is considered an axis-aligned bounding box, and the shortest straight-line distance from each path node to the boundary of this bounding box is calculated. The shortest distances of all nodes form a conflict distance index set. The conflict distance index set is iterated. If any distance value is less than a preset safety threshold of 2.0 meters, the initially selected path is deemed to have a collision risk, triggering a replanning mechanism. Steps S14 and S15 are invoked, the passable area set is updated based on the new obstacle coordinates, a new candidate path sequence is generated, and priorities are ranked. The starting point for the new path search is set to the robot's real-time current position obtained through localization, and the highest priority path in the new ranking is selected as the adjustment path sequence. If all distance values ​​in the conflict distance index set are not less than 2.0 meters, there is no collision risk, and the initially selected path sequence is retained as the adjustment path sequence.

[0119] Finally, path smoothing is performed by fusing the adjusted path sequence with the coordinate data of the new obstacle. Using the occupancy range of the new obstacle as a constraint, a third-order Bézier curve is used to smooth the node coordinates of the adjusted path sequence. The control point selection rules are as follows: for four consecutive nodes in the path, the midpoint between the first and second nodes is taken as the first control point, the second node as the second control point, the third node as the third control point, and the midpoint between the third and fourth nodes as the fourth control point. For the beginning of the path (the first two nodes), the first and second control points are both taken from the starting node, the third control point is taken from the second node, and the fourth control point is taken from the midpoint between the second and third nodes. For the end of the path (the last two nodes), the first control point is taken from the midpoint between the third-to-last and second-to-last nodes, the second control point is taken from the second-to-last node, and the third and fourth control points are both taken from the ending node. During the smoothing process, if the closest distance between a path point on the Bézier curve and the new obstacle is less than 2.5 meters, the control point position is adjusted to move the path point away from the new obstacle, ensuring a sufficient safe distance between the smoothed path and the new obstacle. The path obtained after smoothing is the optimized final path. This path has continuous curvature and a lateral acceleration of less than 0.15g, which meets the robot's motion constraints.

[0120] It should be noted that in the path smoothing process, to ensure that the smoothed curve maintains sufficient safety margin with respect to obstacles at any point, and considering the path shape changes that may be caused by Bézier curve fitting, a value slightly larger than the preset safety threshold (2.0 meters) in the planning stage (2.5 meters) is adopted. This value is set considering that the straight-line connections between the original path nodes have passed the 2.0-meter standard in collision detection, but curve fitting may generate new path points between nodes, and their positions are uncertain. The 0.5-meter margin is intended to offset the small positional deviation introduced by fitting and to provide a buffer for the control error when the robot executes the smoothed curve, thereby further improving the robustness of the final execution path without being overly conservative. This threshold can be adjusted according to the actual control accuracy of the robot and the characteristics of the curve fitting algorithm.

[0121] For example, the first path (with the highest overall priority score) is selected from the path priority ranking as the initial selected path sequence. This path has a total length of 94.8 meters and an average safe distance of 4.1 meters. Through multi-sensor fusion, the environment is continuously monitored during the robot's movement. When the vehicle approaches an intersection, a non-motorized vehicle is detected cutting in laterally from the right. The coordinate data of this new obstacle is obtained: center coordinates (48, 52), occupied area (length 3.5 meters, width 1.8 meters), and orientation angle of 90 degrees. The nearest Euclidean distance between each node in the initially selected path sequence and the new obstacle is calculated, and the 38th to 42nd nodes are identified. The closest distance between nodes is only 1.4 meters, which is less than the preset safety threshold of 2.0 meters, triggering a replanning mechanism. During the replanning process, the set of passable areas is updated in combination with the new obstacle coordinate data, and a new candidate path sequence is generated and sorted. The original third path becomes the new highest priority path and is used as the adjustment path sequence. The adjustment path sequence is fused with the new obstacle coordinate data and smoothed using a third-order Bézier curve. The control point positions are adjusted so that the closest distance between the path point and the new obstacle is not less than 2.5 meters, and finally the optimized final path is obtained. This path has continuous curvature and a lateral acceleration of less than 0.15g.

[0122] In step S17, a motion control command sequence needs to be generated based on the optimized final path, the actual trajectory deviation value is calculated and the motor speed is corrected in real time, and the task completion is determined by the feature point matching rate of the inspection target area, thus obtaining the inspection task termination status, including:

[0123] Based on the optimized final path, a sequence of motion control instructions is generated to drive the motor to perform path tracking and obtain the robot's real-time pose state set.

[0124] Calculate the actual trajectory deviation between the real-time pose state set and the optimized final path. If the actual trajectory deviation is greater than a preset deviation threshold, the motor speed is corrected in real time by a proportional-integral-derivative controller. Obtain the corrected control command sequence and overwrite the motion control command sequence to obtain the compensated execution command sequence.

[0125] The coordinates of feature points in the inspection target area are detected by a visual sensor. The feature point matching rate is calculated and it is determined whether the preset matching rate threshold is reached. If it is reached, a task completion confirmation signal is generated, the execution of the motion control command sequence is stopped, and the inspection task is terminated.

[0126] It should be noted that the motion control command sequence is the core command driving the robot to perform path tracking. It includes front wheel angle and vehicle speed commands, with an output frequency of 50Hz. This frequency ensures smooth motor control and avoids jerking during robot movement. The front wheel angle command is calculated based on the radius of curvature of the path, while the vehicle speed command is dynamically adjusted according to the curvature of the path. The greater the curvature, the lower the vehicle speed. The maximum vehicle speed is set to 18km / h, and the minimum vehicle speed is set to 10km / h to ensure the robot's stability when turning.

[0127] The real-time pose state set is the robot's real-time motion state data, including position coordinates and joint angles. It is obtained by fusing the hub encoder and the inertial measurement unit (IMU), with a sampling frequency of 50Hz and a positioning accuracy better than 3cm, which can accurately reflect the robot's actual motion trajectory.

[0128] The actual trajectory deviation value is a quantitative indicator of the deviation between the robot's actual motion trajectory and the optimized final path. It is obtained by calculating the Euclidean distance between the real-time pose coordinates and the coordinates of the path reference point. The larger the deviation value, the lower the path tracking accuracy. The preset deviation threshold is determined based on the robot's positioning accuracy and path tracking requirements. Based on historical experimental test data, the maximum deviation that ensures the path tracking accuracy meets the inspection task requirements is set as the preset deviation threshold, enabling the robot to accurately travel along the planned path. In this embodiment, it is set to 0.25 meters. This threshold can be adjusted according to the scene's accuracy requirements. For high-precision scenes, it can be reduced to 0.15-0.2 meters; for general scenes, it can be maintained at 0.25 meters; and for low-precision scenes, it can be increased to 0.3-0.4 meters.

[0129] The Proportional-Integral-Derivative (PID) controller is a commonly used closed-loop control algorithm for real-time correction of motor speed and improvement of path tracking accuracy. The parameters of the PID controller (proportional coefficient Kp, integral coefficient Ki, and derivative coefficient Kd) are obtained through the critical proportional gain method. First, Kp is adjusted to produce constant-amplitude oscillations in the system, and the critical proportional coefficient Kp0 and critical oscillation period Tp0 are recorded. Then, based on empirical formulas, Kp = 0.6 × Kp0, Ki = 2 × Kp / Tp0, and Kd = Kp × Tp0 / 8 are calculated. In this example, Kp = 2.8, Ki = 0.12, and Kd = 0.45 are set as an example. This parameter combination can achieve fast, overshoot-free deviation correction. In practice, the specific calculations can be made according to the motion characteristics and path tracking accuracy requirements of the inspection robot.

[0130] The vision sensor uses a binocular camera with a resolution of 1920×1080 and a frame rate of 30Hz to detect feature points in the inspection target area, such as corner points of substation insulator strings and edge points of equipment nameplates. The feature point matching rate is the proportion of detected feature points that match the preset target area's feature points, used to determine whether the robot has reached the inspection target area. The preset matching rate threshold is derived from extensive testing in real-world inspection scenarios, representing the lowest matching rate that ensures the robot accurately reaches the target area. A matching rate lower than the preset threshold can easily lead to misjudgments. In this embodiment, 90% is used as an example. This threshold can be adjusted according to the prominence of the target area's features; it can be reduced to 85% in scenarios with prominent features and increased to 95% in scenarios with less prominent features.

[0131] In this step, the optimized final path is first discretized into a sequence of path reference points at 0.2-meter intervals. Each reference point contains coordinates and curvature information. The front wheel angle is calculated based on the radius of curvature of the path reference points; the smaller the radius of curvature, the larger the front wheel angle. The vehicle speed command is adjusted according to the path curvature: a maximum speed of 18 km / h is used when the radius of curvature is greater than 50 meters, a minimum speed of 10 km / h is used when the radius of curvature is less than 10 meters, and the speed is adjusted linearly for intermediate curvatures. The front wheel angle and speed commands are combined at a frequency of 50 Hz to form a motion control command sequence. The drive motor then performs path tracking and acquires real-time pose. The motion control command sequence is sent to the motor controller via a pulse width modulation (PWM) signal. The motor controller drives the motor to run according to the duty cycle of the PWM signal, performing path tracking. During path tracking, the hub encoder measures the motor's speed and rotation angle in real time to calculate the robot's travel distance and direction; the IMU measures the robot's acceleration and angular velocity in real time. The data from the hub encoder and IMU are fused using a Kalman filter algorithm to obtain the robot's real-time position coordinates and joint angles, forming a real-time pose state set.

[0132] Furthermore, the actual trajectory deviation between the real-time pose state set and the path reference point is calculated. For each path reference point, the corresponding real-time pose data is found, and the Euclidean distance between the two is calculated. If the deviation value is greater than the preset deviation threshold of 0.25 meters, the PID controller is activated to correct the motor speed in real time. The PID controller calculates the motor speed correction amount based on the deviation value, the deviation integral, and the deviation derivative. The correction amount is superimposed on the original vehicle speed command to obtain the corrected control command sequence. This corrected command is then used to overwrite the original motion control command sequence to obtain the compensated execution command sequence. If the deviation value is less than or equal to 0.25 meters, the original motion control command sequence remains unchanged, and the compensated execution command sequence is obtained.

[0133] Finally, the target area is detected and the task is deemed complete. The vision sensor continuously acquires images of the target area. In one possible implementation, the SIFT algorithm can be used to extract feature points from the images. The extracted feature points are matched with a pre-established feature point library, and the Euclidean distance between the extracted feature points and all feature points in the library is calculated. A successful match is considered to be achieved if the distance is less than 1.5 times the minimum distance (experimental testing has verified that when the threshold is set to 1.5 times, it can effectively distinguish between "true matches" and "fuzzy matches," preserving the correct matches to the maximum extent while ensuring matching accuracy). The feature point matching rate is calculated, which is the percentage of successfully matched feature points out of the total number of feature points in the target area. If the matching rate reaches a preset matching rate threshold of 90%, a task completion confirmation signal is generated, a zero-speed command is sent to the motor controller, and the execution of the compensated execution command sequence stops. The robot smoothly stops near the target area, and the inspection task is terminated. If the matching rate does not reach the threshold, the compensated execution command sequence continues to be executed until the matching rate reaches the target or the robot reaches the end of the path.

[0134] It is worth noting that the pre-established feature point library needs to be built before robot deployment. For each target point to be inspected (such as a specific equipment dashboard, valve status indicator, insulator string, etc.), multiple high-resolution images are collected using the vision sensor on the inspection robot under standard lighting, unobstructed, and frontal viewing conditions. Stable and salient feature points and their 128-dimensional descriptors are extracted from these images using the SIFT algorithm. The feature points extracted from multiple images of the same target are clustered and deduplicated, and unstable feature points with large changes in viewing angle are removed. Finally, the feature point set corresponding to the target is formed, which serves as the initial feature point library entry for the target ID. The feature point library is stored in the form of a database. Each record is associated with the target ID, GPS coordinates / map grid number, feature point descriptor set, and environmental metadata at the time of collection (such as approximate lighting conditions). When the robot performs a task, it calls the corresponding feature point set according to the target ID in the path endpoint or task list.

[0135] It should be noted that when the matching rate of a task exceeds 95%, the system will filter out high-quality new feature points extracted from the current frame that have been successfully matched (e.g., points that are stable in the current view but are not in the database) and add them to the database to achieve gradual optimization of the database. If the matching rate of the same target is lower than 80% in three consecutive inspections, the system will generate a "feature database failure" alarm. At this time, manual intervention is required. After confirming that the appearance of the target has changed significantly due to modification, damage, seasonal changes, etc., the image will be re-acquired and the feature point database corresponding to the target will be updated. The entire feature point database will be manually reviewed once a quarter. Based on the feedback from the inspection report, the database will be rebuilt for targets with deteriorating matching performance.

[0136] For example, the system generates a sequence of motion control commands based on the optimized final path, outputting front wheel steering angle and vehicle speed commands at a frequency of 50Hz. A path with a total length of 86.4 meters is discretized into 432 reference points at 0.2-meter intervals. The front wheel steering angle reaches 12.6 degrees at the peak of curvature, and the vehicle speed decreases from 18km / h to 14.3km / h. The system drives the motor to perform path tracking via PWM signals. The robot obtains its current pose every 20ms through the fusion of the wheel hub encoder and IMU, with a positioning accuracy better than 3cm. At the 127th reference point, the lateral deviation is measured. The deviation reached 0.28 meters, exceeding the preset deviation threshold of 0.25 meters. The PID controller (Kp=2.8, Ki=0.12, Kd=0.45) was activated, and the deviation was brought to 0.06 meters within 0.18 seconds. The vision sensor continuously detected the feature points of the inspection target area (substation insulator strings), extracted the feature points and matched them with the preset feature point library. When the matching rate reached 92% (exceeding the 90% threshold), a task completion confirmation signal was generated, and a zero-speed command was sent to the motor controller. The robot smoothly stopped 0.8 meters away from the target, completing the inspection task.

[0137] In summary, this invention discloses a path planning method for inspection robots based on visual intelligence. It collects raw obstacle data through multi-sensor fusion, estimates obstacle states and predicts future trajectories using particle filtering, extracts passable areas based on the predicted trajectories, generates candidate paths using an ant colony optimization algorithm, selects the optimal path through safety screening and priority ranking, dynamically adjusts and smooths the path in conjunction with environmental changes, and finally completes the inspection task through PID control and visual detection. This invention, through the synergistic application of multiple technologies, effectively solves the problems of high collision risk and easy planning failure caused by the uncertainty of obstacle trajectories in dynamic environments, significantly improving the autonomy, safety, and task completion efficiency of inspection robots in complex dynamic scenarios.

[0138] Reference Figure 2 The second embodiment of the present invention provides a path planning system for inspection robots based on visual intelligence, comprising:

[0139] The data acquisition module is used to acquire raw data of moving obstacles in a dynamic environment, and to process the raw data of moving obstacles through particle filtering to obtain the current position and speed information of the obstacles.

[0140] The trajectory prediction module is used to acquire historical trajectory data based on the current position and speed information of the obstacle and perform data fusion, determine abnormal speed changes and update particle weights, and determine the future trajectory of the obstacle through extrapolation calculation.

[0141] The region extraction module is used to determine a set of potential collision regions based on the future trajectory of the obstacle, divide the environment map, and extract a set of passable regions.

[0142] The path generation module is used to simulate path search on the set of passable areas using an ant colony optimization algorithm to generate a set of candidate path sequences.

[0143] The filtering and sorting module is used to calculate the safety distance index matrix and the path length index vector based on the candidate path sequence set, filter the effective path subset and sort it in descending order, and determine the path priority ranking.

[0144] The optimization and adjustment module is used to select the highest priority path in the priority sorting, dynamically obtain the adjustment path sequence according to changes in the surrounding environment, and obtain the optimized final path after smoothing.

[0145] The task execution module is used to generate a motion control command sequence based on the optimized final path, calculate the actual trajectory deviation value and correct the motor speed in real time, and determine whether the task is completed by the feature point matching rate of the inspection target area, thus obtaining the inspection task termination status. It should be noted that the visual intelligence-based inspection robot path planning system provided in this embodiment of the invention is used to execute all the process steps of the visual intelligence-based inspection robot path planning method of the above embodiment. The working principles and beneficial effects of the two are one-to-one, and therefore will not be repeated.

[0146] It should be noted that the system embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Furthermore, in the accompanying drawings of the system embodiments provided by this invention, the connection relationships between modules indicate that they have communication connections, which can be specifically implemented as one or more communication buses or signal lines. Those skilled in the art can understand and implement this without any creative effort.

[0147] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above descriptions are merely specific embodiments of the present invention and are not intended to limit the scope of protection of the present invention. In particular, it should be noted that any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention for those skilled in the art.

Claims

1. A path planning method for an inspection robot based on visual intelligence, characterized in that, include: The raw data of moving obstacles in a dynamic environment is acquired, and the raw data of moving obstacles is processed by particle filtering to obtain the current position and velocity information of the obstacles. Historical trajectory data is obtained based on the current position and speed information of the obstacle and the data is fused. Abnormal speed changes are identified and particle weights are updated. The future trajectory of the obstacle is determined by extrapolation calculation. Based on the future trajectory of the obstacles, a set of potential collision areas is determined, the environmental map is divided, and a set of passable areas is extracted. The ant colony optimization algorithm is used to simulate path search for the set of passable areas, generating a set of candidate path sequences. Based on the candidate path sequence set, calculate the safety distance index matrix and the path length index vector, filter the effective path subset and sort them in descending order, and determine the path priority ranking. The highest priority path in the priority ranking is selected, and the path sequence is dynamically adjusted according to changes in the surrounding environment. After smoothing, the optimized final path is obtained. Based on the optimized final path, a motion control command sequence is generated, the actual trajectory deviation value is calculated, and the motor speed is corrected in real time. The completion status of the inspection task is determined by the feature point matching rate of the inspection target area.

2. The path planning method for inspection robots based on visual intelligence according to claim 1, characterized in that, The process of acquiring raw data of moving obstacles in a dynamic environment, grouping and calculating observation positions, establishing correspondences between obstacles across consecutive frames, and processing the data through particle filtering to determine the current position and velocity information of the obstacles includes: Raw data of moving obstacles in a dynamic environment is obtained through multi-sensor fusion, and the raw data of moving obstacles includes distance data and angle data; The raw data of the moving obstacles is grouped using the DBSCAN clustering algorithm to obtain a first set of points corresponding to multiple independent obstacles; The centroid position, circumscribed rectangle boundary, and orientation of each obstacle are calculated based on the first point set to obtain the observation position of the obstacle in the current frame; Obtain the predicted position distance of the tracked obstacle in the previous frame. If the difference between the predicted position distance and the observed position distance of the obstacle in the current frame is less than a preset distance threshold, then determine the association matching and establish the correspondence between obstacles in consecutive frames. The correspondence between obstacles in the continuous frames is maintained by particle filtering to maintain the particle set. The current position and velocity information of the obstacles are determined by updating the particle weights and resampling.

3. The path planning method for inspection robots based on visual intelligence according to claim 1, characterized in that, The step of acquiring historical trajectory data based on the current position and velocity information of the obstacle and performing data fusion, identifying abnormal velocity changes and updating particle weights, and determining the future trajectory of the obstacle through extrapolation calculation includes: Historical trajectory data is obtained based on the current position and speed information of the obstacle, and the historical trajectory data is fused to obtain a continuous trajectory sequence. Calculate the velocity vector difference between adjacent frames in the continuous trajectory sequence. If the velocity vector difference exceeds a preset vector threshold, it is marked as an abnormal velocity change. If the speed change is abnormal, the particle weight set is updated by particle filtering, and the continuous trajectory sequence is extrapolated based on the updated particle weight set to determine the future trajectory of the obstacle.

4. The path planning method for inspection robots based on visual intelligence according to claim 1, characterized in that, The process of determining a set of potential collision zones based on the future trajectories of the obstacles, dividing the environment map, and extracting a set of passable areas includes: From the future trajectory of the obstacle, obtain a set of all predicted location points, and extend a safe distance for each predicted location point to obtain a set of potential collision areas; Based on the set of potential collision regions, an environmental map is obtained and divided into uniform grid cells. It is determined whether each grid cell overlaps with the set of potential collision regions. The overlapping grid cells are marked as occupied grids to form an occupied grid set. The occupied grid set is subjected to morphological dilation, and connected components are marked to obtain a set of independent obstacle regions; The remaining grids in the environment map, excluding the set of independent obstacle areas, are assessed for connectivity, and connected grids are merged to obtain a set of passable areas.

5. The path planning method for inspection robots based on visual intelligence according to claim 1, characterized in that, The step of simulating path search using an ant colony optimization algorithm to generate a candidate path sequence set for the set of passable areas includes: Obtain the starting grid and ending grid from the set of passable areas, generate a path node graph, and initialize the pheromone concentration matrix of the ant colony optimization algorithm to obtain the initial set of ant colony individuals; Based on the initial set of individual ant colonies, simulate path movement on the path node graph to obtain a single-round path node sequence set; For the single-round path node sequence set, collision detection is used to filter out collision-free path node sequences, and the path heuristic distance is evaluated to obtain a path quality score set; The pheromone concentration matrix is ​​updated based on the path quality score set, and after iterating through a preset number of rounds, high-concentration paths are extracted to generate a candidate path sequence set.

6. The path planning method for inspection robots based on visual intelligence according to claim 1, characterized in that, The step of calculating the safety distance index matrix and path length index vector based on the candidate path sequence set, filtering the effective path subset and sorting it in descending order, and determining the path priority ranking includes: Obtain the node coordinate sequence of each path from the candidate path sequence set, calculate the Euclidean distance set with the nearest occupied grid boundary, and form a safety distance index matrix; Determine whether there is a distance value in the safety distance index matrix that is less than a preset safety threshold. If so, mark the path corresponding to the distance value as an invalid path to obtain a subset of valid paths. Calculate the path length index vector of the effective path subset, and weight and fuse the average value of the path length index vector and the safety distance index matrix to generate a comprehensive priority score vector; The effective path subset is sorted in descending order based on the comprehensive priority score vector to determine the path priority ranking.

7. The path planning method for inspection robots based on visual intelligence according to claim 1, characterized in that, The process of selecting the highest priority path in the priority ranking, dynamically obtaining and adjusting the path sequence based on changes in the surrounding environment, and then smoothing the result to obtain the optimized final path includes: The highest priority path in the priority ranking is selected as the initial selected path sequence, and the changes in the surrounding environment of the initial selected path sequence are monitored to obtain new obstacle coordinate data. Calculate the nearest Euclidean distance between the new obstacle coordinate data and each node of the initially selected path sequence. If there is a distance value less than a preset safety threshold, trigger replanning to generate a new path priority sort and select the highest priority path as the adjusted path sequence. If there is no such path, retain the initially selected path sequence as the adjusted path sequence. The adjusted path sequence is fused with the new obstacle coordinate data, and the node coordinates of the adjusted path sequence are smoothed using Bézier curves to obtain the optimized final path.

8. The path planning method for inspection robots based on visual intelligence according to claim 1, characterized in that, The process of generating a motion control command sequence based on the optimized final path, calculating the actual trajectory deviation value and correcting the motor speed in real time, determining whether the task is completed by checking the feature point matching rate of the inspection target area, and obtaining the inspection task termination status includes: Based on the optimized final path, a sequence of motion control instructions is generated to drive the motor to perform path tracking and obtain the robot's real-time pose state set. Calculate the actual trajectory deviation between the real-time pose state set and the optimized final path. If the actual trajectory deviation is greater than a preset deviation threshold, the motor speed is corrected in real time by a proportional-integral-derivative controller. Obtain the corrected control command sequence and overwrite the motion control command sequence to obtain the compensated execution command sequence. The coordinates of feature points in the inspection target area are detected by a visual sensor. The feature point matching rate is calculated and it is determined whether the preset matching rate threshold is reached. If it is reached, a task completion confirmation signal is generated, the execution of the motion control command sequence is stopped, and the inspection task is terminated.

9. A path planning system for an inspection robot based on visual intelligence, characterized in that, include: The data acquisition module is used to acquire raw data of moving obstacles in a dynamic environment, and to process the raw data of moving obstacles through particle filtering to obtain the current position and speed information of the obstacles. The trajectory prediction module is used to acquire historical trajectory data based on the current position and speed information of the obstacle and perform data fusion, determine abnormal speed changes and update particle weights, and determine the future trajectory of the obstacle through extrapolation calculation. The region extraction module is used to determine a set of potential collision regions based on the future trajectory of the obstacle, divide the environment map, and extract a set of passable regions. The path generation module is used to simulate path search on the set of passable areas using an ant colony optimization algorithm to generate a set of candidate path sequences. The filtering and sorting module is used to calculate the safety distance index matrix and the path length index vector based on the candidate path sequence set, filter the effective path subset and sort it in descending order, and determine the path priority ranking. The optimization and adjustment module is used to select the highest priority path in the priority sorting, dynamically obtain the adjustment path sequence according to changes in the surrounding environment, and obtain the optimized final path after smoothing. The task execution module is used to generate a motion control command sequence based on the optimized final path, calculate the actual trajectory deviation value and correct the motor speed in real time, determine whether the task is completed by the feature point matching rate of the inspection target area, and obtain the inspection task termination status.

Images