A sensing optical cable laying robot path planning and obstacle avoidance system and method thereof

Abstract

The application discloses a kind of sensing optical cable laying robot path planning and obstacle avoidance system and method thereof, belong to robot path planning technical field.The system includes environment perception, data preprocessing, path planning, obstacle avoidance control and post-processing module.Method is obtained environment information by laser radar and camera and pre-processing, based on improved dynamic RRT algorithm carries out path planning, introduces adaptive step adjustment mechanism, according to environment complexity, obstacle distance and path curvature Dynamic adjustment step, combine real-time obstacle detection and dynamic replanning function Generation smooth path;PID control algorithm is used to adjust the speed and steering angle of robot to realize obstacle avoidance, and the post-processing module optimizes the evaluation path.The application improves the real-time performance, path quality and environmental adaptability of the robot in complex dynamic environment, ensures efficient, safe and accurate laying of optical cable.

Description

Technical Field

[0001] This invention relates to the field of robot path planning and obstacle avoidance technology, and in particular to a path planning and obstacle avoidance system and method for a sensor-guided optical cable laying robot. Background Technology

[0002] Laying optical sensing cables is a crucial task in fiber optic monitoring of slope deformation in open-pit mines, playing a decisive role in improving the sensitivity, stability, signal-to-noise ratio, and effective monitoring length of the monitoring system. Optical sensing cables can be laid in various ways, including overhead and underground. Underground laying is primarily used for open-pit mine slope deformation monitoring, and can be further subdivided into trench laying, conduit laying, and direct burial, depending on the specific conditions. Due to the steep slopes and unstable soil conditions of open-pit mine slopes, and the heavy weight and long distances of armored optical sensing cables, traditional manual cable laying methods are not only inefficient but also pose significant safety risks.

[0003] Optical cable laying robots use a conveyor mechanism to move the optical cable along a designated path, reducing the risks and costs of manual laying and improving the efficiency and safety of optical cable laying to some extent. However, when faced with the complex terrain of open-pit mine slopes, the existing optical cable laying robots lack flexible path planning, making it difficult to effectively avoid obstacles while ensuring the straightness of the optical cable. The bending and twisting of the optical cable can easily lead to a decrease in sensing performance, affecting the accuracy of slope deformation monitoring and failing to meet the needs of efficient and accurate laying of sensing optical cables.

[0004] Existing methods for path planning and obstacle avoidance in fiber optic cable laying robots mainly suffer from the following problems:

[0005] (1) Insufficient real-time performance: In the dynamically changing underground environment, the position and shape of obstacles may change at any time. In construction areas, obstacles may suddenly appear due to construction activities or the original position of obstacles may shift. Traditional path planning algorithms often struggle to replan a suitable path in a short time when faced with such situations. For example, although Algorithm A can find a path relatively quickly under the guidance of heuristic search, it is sensitive to the choice of heuristic function and performs poorly in dynamic environments. Once the environment changes, the entire path needs to be replanned, resulting in a large computational cost. Although Dijkstra's algorithm can guarantee finding the shortest path, its computational cost is enormous and it cannot be applied in real time in large-scale dynamic environments.

[0006] (2) Path quality needs improvement: Although some algorithms can find feasible paths, the paths are often redundant and not smooth enough. For example, some grid-based path planning algorithms may generate polyline paths, requiring the robot to turn frequently during travel. This not only increases energy consumption and mechanical wear, but may also cause tension changes during the fiber optic cable laying process, affecting the laying quality and service life of the fiber optic cable. In addition, frequent turning may also reduce the robot's travel speed, thereby reducing the efficiency of fiber optic cable laying.

[0007] (3) Poor adaptability to environmental complexity: Traditional algorithms lack adaptive adjustment capabilities when facing underground environments of varying complexity. For example, in narrow trenches with dense obstacles, more refined path planning and more cautious obstacle avoidance strategies are required; while in relatively open underground spaces or relatively flat direct burial areas, more efficient planning methods can be used to accelerate task progress. However, traditional path planning algorithms typically employ fixed parameters and planning strategies, which cannot be dynamically adjusted according to the complexity of the environment. This leads to low planning efficiency and increased computation time in complex environments; while in simple environments, the robot's performance cannot be fully utilized, affecting the overall efficiency of fiber optic cable laying. Summary of the Invention

[0008] The purpose of this invention is to provide a path planning and obstacle avoidance system and method for a sensor-based optical cable laying robot, which improves the robot's real-time performance, path quality, and environmental adaptability in complex dynamic environments, and ensures efficient, safe, and accurate optical cable laying.

[0009] To achieve the above objectives, the present invention provides a path planning and obstacle avoidance system for a sensor-guided optical cable laying robot, comprising:

[0010] The environmental perception module, consisting of lidar and cameras, is used to acquire information about the underground environment in real time, including the location, shape, and size of obstacles, as well as the layout of the environment.

[0011] The data preprocessing module is used to preprocess the received point cloud data from the lidar and the image data from the camera, wherein the point cloud data is filtered and downsampled, and the image data is grayscaled and edge detected.

[0012] The path planning module, based on an improved dynamic RRT algorithm, performs path planning according to preprocessed environmental information; it introduces an adaptive step size adjustment mechanism and performs real-time obstacle detection and dynamic replanning.

[0013] The obstacle avoidance control module is used to control the robot's movement according to the path generated by the path planning module. It adjusts the robot's speed and turning angle to make the robot travel along the planned path and makes fine adjustments to the robot's movement based on real-time feedback of environmental information.

[0014] The post-processing module is used to optimize and evaluate the path planning results and obstacle avoidance control process, smooth the planned path, evaluate the safety and feasibility of the path, and record the robot's driving trajectory and obstacle avoidance information.

[0015] The lidar measures the distance and angle of surrounding objects by emitting laser pulses and receiving reflected signals, generating point cloud data; the camera collects image information of the environment to help identify the type and characteristics of obstacles.

[0016] Preferably, the lidar measures the distance and angle information of surrounding objects by emitting laser pulses and receiving reflected signals, generating point cloud data; the camera collects image information of the environment to assist in identifying the type and characteristics of obstacles.

[0017] Preferably, the data preprocessing module uses a voxel-based filtering method on the point cloud data to divide the three-dimensional space into a voxel grid of a certain size, and replaces the points in each voxel grid with their average value; for the image data, the RGB image is first converted into a grayscale image, and then edge detection is performed on the grayscale image, using the Canny operator to extract the edge information in the image.

[0018] A path planning and obstacle avoidance method for a sensor-operated optical cable laying robot, implemented in the aforementioned path planning and obstacle avoidance system for a sensor-operated optical cable laying robot, comprises the following steps:

[0019] S1. Environmental perception: Acquire point cloud data and image data of the underground environment through lidar and cameras;

[0020] S2. Data preprocessing: Filtering and downsampling are performed on the point cloud data, and grayscale and edge detection are performed on the image data;

[0021] S3. Path planning: Based on the improved dynamic RRT algorithm, path planning is performed according to the preprocessed environmental information. An adaptive step size adjustment mechanism is introduced, and real-time obstacle detection and dynamic replanning are performed.

[0022] S4. Obstacle Avoidance Control: Controls the robot's movement according to the planned path, adjusts the robot's speed and turning angle, so that the robot travels along the planned path, and makes fine adjustments to the robot's movement based on real-time feedback of environmental information.

[0023] S5. Post-processing: Optimize and evaluate the path planning results and obstacle avoidance control process, smooth the planned path, evaluate the safety and feasibility of the path, and record the robot's driving trajectory and obstacle avoidance information.

[0024] S6. Output Results: Output the final planned path and obstacle avoidance control information to the robot's execution system and display the results in a visual manner on the monitoring screen.

[0025] Preferably, the specific steps of path planning in S3 are as follows:

[0026] S3.1 Initialization: Set the robot's starting position, target position, and operating environment range, and initialize the parameters of the improved dynamic RRT algorithm, including the starting position as the root node of the tree, the initial value of the exploration step size, the initial value of the expansion number, the step size adjustment coefficient based on the environmental complexity, and the dynamic replanning trigger threshold.

[0027] S3.2 Constructing the RRT tree: Starting from the root node, randomly generate the target point; assess the current environment complexity by calculating the number of obstacles per unit volume, and adjust the exploration step size according to the environment complexity; find the node in the tree that is closest to the target point, and grow a new node from that node towards the target point with the adjusted step size. Repeat this process until the preset number of expansions is reached or the nodes of the tree are close to the target position. During the expansion process, record the node generated by each expansion and its parent node information.

[0028] S3.3 Path Extraction: When a node in the tree is close to the target location, a preliminary path is extracted by backtracking from the nodes near the target location to the root node; the preliminary path is then optimized, including removing redundant nodes and using a smoothing algorithm to smooth the path to form a smooth path.

[0029] S3.4 Real-time obstacle detection and dynamic replanning: As the robot travels along the planned path, the environmental perception module acquires information about the surrounding environment in real time. If the distance between the detected obstacle and the robot is less than the dynamic replanning trigger threshold, the newly detected obstacle information is updated in the environmental model. The current position of the robot is used as the new starting position, the target position remains unchanged, and the above path planning process is re-executed to generate a new obstacle avoidance path.

[0030] Preferably, the adaptive step size adjustment mechanism in S3 includes the following adjustment strategies:

[0031] Step size adjustment strategy based on environmental complexity: The environmental complexity is determined by calculating the number of obstacles within a preset volume range around the robot. When the number of obstacles per unit volume exceeds a preset threshold, it is considered high environmental complexity; otherwise, it is considered low environmental complexity. Based on the environmental complexity, the step size is adjusted using the following formula:

[0032]

[0033] in, The adjusted step size, The step size before adjustment is given, and k is the step size adjustment coefficient. When the environmental complexity is high, k is the step size reduction coefficient. When the environmental complexity is low, k is the step size increment factor and ;

[0034] Step size adjustment strategy based on obstacle distance: The step size is dynamically adjusted according to the distance between the robot and the nearest obstacle. The step size increases as the distance between the robot and the nearest obstacle increases and decreases as the distance between the robot and the nearest obstacle decreases. The adjustment formula is as follows:

[0035]

[0036] in, Based on step size, This is the step size adjustment range. For reference distance, The distance between the robot and the nearest obstacle;

[0037] Step size adjustment strategy based on path curvature: On curved path segments, the step size is adjusted according to the radius of curvature of the current path. The adjustment formula is as follows:

[0038]

[0039] in, This represents the robot's maximum permissible step size in the current environment. The radius of curvature of the current path. Let the minimum radius of curvature that the robot can safely pass through be calculated. Let the adjusted step size be .

[0040] Preferably, the specific steps of obstacle avoidance control in S4 are as follows:

[0041] S4.1 Deviation Calculation: Real-time acquisition of the lateral deviation and deviation change rate of the robot's current position relative to the planned path, where the lateral deviation is the vertical distance from the robot's actual position to the planned path, and the deviation change rate is the change in lateral deviation per unit time.

[0042] S4.2 Control Quantity Calculation: Using the PID control algorithm, the output of the PID controller is calculated based on the lateral deviation and the rate of change of deviation: The calculation formula is as follows;

[0043]

[0044] Where u is the output of the PID controller. This is the proportionality coefficient. The integral coefficient is... Here, denoted as the differential coefficient, e is the lateral deviation, ∫e dt is the integral term of the lateral deviation, and de / dt is the rate of change of the lateral deviation.

[0045] S4.3 Steering Angle Adjustment: Based on the calculated output of the PID controller and the gain of the robot steering system, determine the steering angle adjustment value to make the robot turn to the inside or outside of the path to correct the deviation and return to the planned path;

[0046] S4.4 Speed ​​Adjustment: The travel speed is dynamically adjusted based on the radius of curvature of the planned path and the robot's kinematic model. A smaller radius of curvature results in a lower travel speed, and a larger radius of curvature results in a higher travel speed. The speed adjustment must not exceed the robot's maximum permissible speed. The robot's kinematic model is represented as follows:

[0047]

[0048] in, The adjusted speed, The maximum speed of the robot, It is the minimum radius of curvature that the robot can safely pass through at maximum speed.

[0049] Preferably, in S3.3 and S5, the path smoothing process adopts a curve fitting algorithm, which includes the Bézier curve algorithm.

[0050] According to specific embodiments provided by the present invention, the present invention discloses the following technical effects:

[0051] (1) Improve the adaptability and real-time performance of dynamic environments

[0052] The system captures changes in obstacles in real time through the environmental perception module. Combined with a dynamic replanning mechanism, when the distance between the obstacle and the robot is less than the trigger threshold, it quickly reconstructs the path from the current position as a new starting point. This avoids the high computational cost of full path replanning in traditional algorithms, ensures rapid response to environmental changes in complex and dynamic underground environments, significantly reduces the risk of collisions, and ensures the safe operation of the robot.

[0053] (2) Optimize path quality and laying efficiency

[0054] The adaptive step size adjustment mechanism dynamically adjusts the exploration step size based on environmental complexity, obstacle distance, and path curvature. In complex environments, the step size is reduced to improve planning accuracy, while in open environments, the step size is increased to improve exploration efficiency. At the same time, through path smoothing, redundant nodes and corners are removed, reducing frequent robot turns, lowering energy consumption and mechanical wear, avoiding the degradation of sensing performance of optical cables due to bending and twisting, improving the straightness and quality of laying, and extending the service life of optical cables.

[0055] (3) Enhance control accuracy and stability

[0056] The obstacle avoidance control module adopts a PID control algorithm, which calculates the lateral deviation and rate of change in real time to precisely adjust the steering angle and driving speed, enabling the robot to stably track the planned path. The speed adjustment is combined with the path curvature and kinematic model to ensure safety and stability when turning, further improving the accuracy of fiber optic cable laying.

[0057] (4) Achieve full-process intelligence and data support

[0058] The entire process, from environmental perception, data preprocessing, and path planning to obstacle avoidance control and post-processing, is automated, reducing human intervention and mitigating operational risks in dangerous scenarios such as open-pit mine slopes. The post-processing module records and analyzes path performance and driving data, providing data support for algorithm iteration and task optimization, and continuously improving system performance.

[0059] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0060] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0061] Figure 1 This is a schematic diagram of the framework of an embodiment of the path planning and obstacle avoidance system for a sensor-driven optical cable laying robot according to the present invention.

[0062] Figure 2 This is a schematic diagram of the process structure of an embodiment of the path planning and obstacle avoidance method for a sensor-driven optical cable laying robot according to the present invention.

[0063] Figure 3 This is a schematic diagram of the path planning process according to an embodiment of the present invention.

[0064] Figure 4 This is a schematic diagram of the obstacle avoidance control process according to an embodiment of the present invention. Detailed Implementation

[0065] 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 of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0066] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0067] Example

[0068] like Figure 1 As shown, a path planning and obstacle avoidance system for a sensor-guided optical cable laying robot includes:

[0069] The environmental perception module, consisting of a lidar and a camera, is used to acquire information about the underground environment in real time, including the location, shape, and size of obstacles, as well as the layout of the environment. The lidar measures the distance and angle of surrounding objects by emitting laser pulses and receiving reflected signals, generating point cloud data. The camera collects image information of the environment to help identify the type and characteristics of obstacles.

[0070] The data preprocessing module is used to preprocess the received LiDAR point cloud data and camera image data. Specifically, it performs filtering and downsampling operations on the point cloud data. The preprocessing module uses a voxel-based filtering method to divide the three-dimensional space into a voxel grid of a certain size, replacing the points within each voxel grid with their average value. For the image data, it performs grayscale conversion and edge detection processing to enhance image contrast and edge information, improving data quality and usability. The image data is first converted from RGB to grayscale, and then edge detection is performed on the grayscale image, using the Canny operator to extract edge information.

[0071] The path planning module, based on an improved dynamic RRT algorithm, plans paths according to preprocessed environmental information. This module introduces an adaptive step size adjustment mechanism, dynamically adjusting the exploration step size based on environmental complexity to improve algorithm efficiency and path quality. Simultaneously, it features real-time obstacle detection and dynamic replanning capabilities; once a new obstacle or environmental change is detected, the path is immediately replanned to ensure the robot can safely and efficiently avoid obstacles.

[0072] The obstacle avoidance control module controls the robot's movement based on the path generated by the path planning module, thus achieving obstacle avoidance. By adjusting the robot's speed and steering angle, the robot travels along the planned path. Simultaneously, based on real-time environmental feedback, the module fine-tunes the robot's movement to ensure safe operation in dynamic environments.

[0073] The post-processing module optimizes and evaluates the path planning results and obstacle avoidance control process. It smooths the planned path to reduce redundancy and frequent turns; assesses the safety and feasibility of the path, adjusting and optimizing potentially risky path segments; and records the robot's trajectory and obstacle avoidance information to provide a reference for subsequent task planning and algorithm improvement.

[0074] A path planning and obstacle avoidance method for a sensor-based optical cable laying robot, implemented in the aforementioned path planning and obstacle avoidance system for a sensor-based optical cable laying robot, such as... Figure 2 As shown, the steps are as follows:

[0075] S1. Environmental perception: A lidar and a camera are installed at the front end of the optical cable laying robot. The lidar emits laser pulses at a frequency of 10Hz and receives reflected signals to obtain point cloud data of the underground environment; the camera collects image data of the robot's surroundings at a frame rate of 30fps.

[0076] S2. Data Preprocessing: For point cloud data, a voxel-based filtering method is used to divide the 3D space into a voxel grid of a certain size, such as 0.05m × 0.05m × 0.05m. Points within each voxel grid are replaced with their average value to remove noise and invalid points. Assuming the original point cloud data has 200,000 points, after filtering, 150,000 valid points are retained. Then, downsampling is performed to reduce the density of the point cloud data to 100 points per cubic meter, reducing the data volume and improving the efficiency of subsequent processing. For image data, RGB images are converted to grayscale images, and then edge detection is performed on the grayscale images. The Canny operator is used to extract edge information from the image, enhancing the image's contour features.

[0077] S3. Path Planning: Based on an improved dynamic RRT algorithm, path planning is performed according to preprocessed environmental information. An adaptive step size adjustment mechanism is introduced, and real-time obstacle detection and dynamic replanning are conducted. Figure 3 As shown, the specific steps of path planning are as follows:

[0078] S3.1 Initialization: Set the robot's starting position origin O (0, 0, 0) and target position T (10, 10, 0) in a 10m × 10m × 3m surface trench environment. Initialize the relevant parameters of the improved dynamic RRT algorithm, including the root node of the tree (starting position O), the initial exploration step size d = 0.5m, the initial number of expansions N = 100, the adaptive step size adjustment coefficient k, and the dynamic replanning trigger threshold s = 2m (dynamic replanning is triggered when the distance between the obstacle and the robot is less than 2m).

[0079] S3.2 Construct the RRT tree starting from the root node O, and randomly generate a target point Prand. Calculate the current environment complexity by evaluating the number of obstacles per unit volume. For example, during the first expansion, if the LiDAR and camera detect 6 obstacles distributed within a 5 cubic meter area, the number of obstacles per unit volume is 6 / 5 = 1.2 obstacles / m. 3Based on a preset threshold (e.g., the number of obstacles per unit volume is greater than 1 / m), 3 If the environment is deemed to be highly complex, the exploration step size d is adjusted accordingly. Find the node in the tree that is closest to the target point Prand. ,from Towards New nodes grow This generates new branches. Assume... arrive The distance is 1.2 m, and with the adjusted step size of 0.4 m, from... Towards Move the tree 0.4 m in the direction to obtain Pnew. Repeat this process until the preset number of expansions N=100 or the tree node approaches the target position T. During the expansion process, record the node generated in each expansion and its parent node information to construct the RRT tree.

[0080] S3.3 Path Extraction: When a node in the tree approaches the target position T (e.g., the distance to T is less than 0.5m), starting from the node Pgoal near the target position, backtracking to the root node O, a preliminary path is extracted. The preliminary path may contain some redundant nodes, such as three consecutive nodes A → B → C, where the directions from B to A and B to C are almost opposite, with obvious bends. The preliminary path is optimized, including removing redundant nodes and smoothing the path. A Bézier curve is used to smooth the path, making the robot's trajectory smoother. Assuming the preliminary path has 20 nodes, after removing redundant nodes, 15 nodes remain, and then a smooth path is obtained through Bézier curve smoothing.

[0081] S3.4 Real-time Obstacle Detection and Dynamic Replanning: During the robot's journey along the planned path, the environmental perception module acquires real-time information about the surrounding environment. Assume that when the robot reaches point P1 (2, 2, 0) on the path, the lidar detects an obstacle 1.5m ahead (point...). A pipe support with a diameter of 0.8m suddenly appears, and the camera confirms its presence. The current distance between the obstacle and the robot is calculated to be 1.5m, which is less than the dynamic replanning trigger threshold s=2m. Therefore, the dynamic replanning mechanism is immediately activated. The newly detected obstacle... The information is updated in the environment model, using the robot's current position P1 as the new starting position, with the target position remaining T(10, 10, 0). The path planning process described above is then re-executed to generate a new obstacle avoidance path. During the replanning process, the RRT tree will avoid obstacles. We need to find new feasible paths. Assume that after replanning, we obtain a path that bypasses point P1. The new path to T is 1.2m longer than the original path, but it ensures that the robot can safely avoid obstacles.

[0082] The adaptive step size adjustment mechanism includes the following adjustment strategies:

[0083] Step size adjustment strategy based on environmental complexity: First, assess the environmental complexity by calculating the number of obstacles within a preset volume range around the robot. If the number of obstacles per unit volume exceeds a preset threshold, it is considered high environmental complexity; otherwise, it is considered low environmental complexity. Based on the environmental complexity, adjust the step size using the following formula:

[0084]

[0085] in, The adjusted step size, The step size before adjustment is given, and k is the step size adjustment coefficient. When the environmental complexity is high, k is the step size reduction coefficient. For example, if the current step size is 0.5m, and the environment complexity is high, then k=0.7, and the adjusted step size is... When the environmental complexity is low, k is the step size increment factor and For example, if the current step size is 0.35m and the environmental complexity is low, we can take k=1.5, then the adjusted step size will be... .

[0086] Step size adjustment strategy based on obstacle distance: The step size is dynamically adjusted according to the distance between the robot and the nearest obstacle. The step size increases as the distance between the robot and the nearest obstacle increases and decreases as the distance between the robot and the nearest obstacle decreases. The adjustment formula is as follows:

[0087]

[0088] in, Based on step size, This is the step size adjustment range. For reference distance, The distance between the robot and the nearest obstacle; for example, , , Distance between the robot and the nearest obstacle The adjusted step size is .

[0089] Step size adjustment strategy based on path curvature: On curved path segments, the step size is adjusted according to the radius of curvature of the current path. The adjustment formula is as follows:

[0090]

[0091] in, This represents the robot's maximum permissible step size in the current environment. The radius of curvature of the current path. Let the minimum radius of curvature that the robot can safely pass through be calculated. Let the adjusted step size be .For example, , The radius of curvature of the current path segment The step size is then adjusted to .

[0092] S4. Obstacle Avoidance Control: Controls the robot's movement according to the planned path, adjusting the robot's speed and steering angle to ensure the robot travels along the planned path, and fine-tunes the robot's movement based on real-time environmental feedback. For example... Figure 4 As shown, the specific steps of obstacle avoidance control are as follows:

[0093] S4.1 Deviation Calculation: Real-time acquisition of the lateral deviation and rate of change of the robot's current position relative to the planned path. The lateral deviation is the vertical distance from the robot's actual position to the planned path, and the rate of change is the change in lateral deviation per unit time. Assume there are three consecutive nodes on the planned path: P1 (2, 2, 0), P2 (3, 3, 0), and P3 (4, 4, 0). The robot is at P1 and needs to move to P2. The direction vector of the path P1→P2 is (1, 1, 0), the angle between the robot's current orientation and the path direction is 0 degrees, and the lateral deviation e = 0m.

[0094] S4.2 Control Quantity Calculation: Using the PID control algorithm, the output of the PID controller is calculated based on the lateral deviation and the rate of change of deviation: The calculation formula is as follows;

[0095]

[0096] Where u is the output of the PID controller. This is the proportionality coefficient. The integral coefficient is... Let be the differential coefficient, e be the lateral deviation, ∫e dt be the integral term of the lateral deviation, and de / dt be the rate of change of the lateral deviation. Integral coefficient Differential coefficients Assume that at t=0, the lateral deviation e(0)=0.0m; at t=1s, due to environmental disturbances or other factors, the lateral deviation becomes e(1)=0.1m, and the rate of change of the lateral deviation is de / dt=0.1 m / s². Substituting into the above formula, the output of the PID controller is u=0.175.

[0097] S4.3 Steering Angle Adjustment: Based on the calculated output u of the PID controller and the robot steering system gain, determine the steering angle adjustment value δ. Assume the robot steering system gain is G = 0.5 rad⁻¹, and the steering angle... °, causing the robot to turn to the inside or outside of the path to correct deviations and return to the planned path.

[0098] S4.4 Speed ​​Adjustment: The travel speed is dynamically adjusted based on the radius of curvature of the planned path and the robot's kinematic model. A smaller radius of curvature results in a lower travel speed, and a larger radius of curvature results in a higher travel speed. The speed adjustment must not exceed the robot's maximum permissible speed. The robot's kinematic model is represented as follows:

[0099]

[0100] in, The adjusted speed, The maximum speed of the robot, This is the minimum radius of curvature that the robot can safely pass through at its maximum speed. For example, set... When the robot travels to a section of the path with a radius of curvature of 2m, its speed is adjusted to... This ensures the robot's stability and safety when turning.

[0101] S5. Post-processing: The actual path traveled by the robot is further optimized, taking into account factors such as path length, smoothness, and safety distance. The effectiveness of path planning and obstacle avoidance control is evaluated, including path feasibility and safety, robot driving efficiency, and fiber optic cable laying quality. Data such as the robot's trajectory, obstacle avoidance information, and environmental characteristics are recorded.

[0102] S6. Output Results: The final planned path and obstacle avoidance control information are output to the robot's execution system in a specific manner. The execution system uses this information to control the robot's movement and complete the fiber optic cable laying task. Simultaneously, the path planning and obstacle avoidance results are displayed visually on the monitoring screen, allowing operators to understand the robot's working status and environmental information in real time. For example, the monitoring screen can display the robot's current position, planned path, obstacle avoidance area, and obstacle locations, distinguished by different colors and icons, enabling operators to intuitively observe the robot's movement.

[0103] The remaining technical features in the above embodiments can be flexibly selected by those skilled in the art to meet different specific practical needs according to actual circumstances. Modifications and variations made by those skilled in the art that do not depart from the spirit and scope of the present invention should be within the protection scope of the appended claims. In the above description, numerous specific details have been set forth to provide a thorough understanding of the present invention. However, it will be apparent to those skilled in the art that these specific details are not necessary to implement the present invention. In other instances, to avoid obscuring the present invention, well-known techniques, such as specific construction details, operating conditions, and other technical conditions, have not been specifically described.

[0104] This document uses specific examples to illustrate the principles and implementation methods of the present invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of the present invention. Furthermore, those skilled in the art will recognize that, based on the ideas of the present invention, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of the present invention.

Claims

1. A sensing optical cable laying robot path planning and obstacle avoidance system, characterized in that, Comprise: An environmental perception module composed of a laser radar and a camera, for real-time acquisition of information of the underground environment, including the position, shape, size of obstacles and the layout of the environment; A data preprocessing module for preprocessing the received point cloud data of the laser radar and the image data of the camera, wherein the point cloud data is filtered and down-sampled, and the image data is grayed and edge detected; A path planning module based on an improved dynamic RRT algorithm for path planning according to the preprocessed environmental information; An adaptive step adjustment mechanism is introduced, and real-time obstacle detection and dynamic replanning are performed; The adaptive step adjustment mechanism includes the following adjustment strategies: An environmental complexity-based step adjustment strategy: the number of obstacles within a preset volume range around the robot is calculated to determine the environmental complexity, and when the number of obstacles per unit volume exceeds a preset threshold, it is determined as high environmental complexity, otherwise it is determined as low environmental complexity, based on the environmental complexity, the step is adjusted using the following formula: ； wherein, is the adjusted step size, is the unadjusted step size, k is a step size adjustment factor, when the environmental complexity is high, k is a step size reduction factor and when the environmental complexity is low, k is a step size increase factor and ; An obstacle distance-based step adjustment strategy: the step is dynamically adjusted according to the distance between the robot and the nearest obstacle, the step increases as the distance between the robot and the nearest obstacle increases, and the step decreases as the distance between the robot and the nearest obstacle decreases, the adjustment formula is: ； wherein, is a base step size, is a step size adjustment magnitude, is a reference distance, is a distance of the robot to the nearest obstacle; A path curvature-based step adjustment strategy: in a curved path segment, the step is adjusted according to the curvature radius of the current path, the adjustment formula is: ； wherein, is the maximum allowed step size for the robot in the current environment, is the radius of curvature of the current path, is the minimum radius of curvature that the robot can safely pass through, if the calculated is greater than the minimum radius of curvature that the robot can safely pass through, then the adjusted step size is ; An obstacle avoidance control module for controlling the motion of the robot according to the path generated by the path planning module, adjusting the speed and steering angle of the robot to make it travel along the planned path, and fine-tuning the robot's motion according to real-time feedback of environmental information; A post-processing module for optimizing and evaluating the path planning results and obstacle avoidance control process, smoothing the planned path, evaluating the safety and feasibility of the path, and recording the robot's travel trajectory and obstacle avoidance information.

2. The sensing optical cable laying robot path planning and obstacle avoidance system according to claim 1, characterized in that: The laser radar measures the distance and angle information of surrounding objects by emitting laser pulses and receiving reflected signals, generating point cloud data; the camera collects image information of the environment to assist in identifying the type and characteristics of obstacles.

3. The sensing optical cable laying robot path planning and obstacle avoidance system according to claim 2, characterized in that: The data preprocessing module uses a voxel-based filtering method for the point cloud data, dividing the three-dimensional space into a certain size of voxel grid, and replacing the points in each voxel grid with their average value; For the image data, first convert the RGB image to a grayscale image, then perform edge detection on the grayscale image, and use the Canny operator to extract edge information from the image.

4. A method for path planning and obstacle avoidance of a sensing optical cable laying robot, executed in a system for path planning and obstacle avoidance of a sensing optical cable laying robot according to any one of claims 1-3, characterized in that, The steps are as follows: S1, environmental perception: acquire point cloud data and image data of the underground environment through the laser radar and the camera; S2, data preprocessing: filter and down-sample the point cloud data, and gray and edge detect the image data; S3, path planning: based on an improved dynamic RRT algorithm, path planning is performed according to the preprocessed environmental information, an adaptive step adjustment mechanism is introduced, and real-time obstacle detection and dynamic replanning are performed; The adaptive step adjustment mechanism includes the following adjustment strategies: Step length adjustment strategy based on environmental complexity: Determine the environmental complexity by calculating the number of obstacles within a preset volume range around the robot. If the number of obstacles per unit volume exceeds a preset threshold, it is determined as high environmental complexity, otherwise it is determined as low environmental complexity. Based on the environmental complexity, adjust the step length using the following formula: ； wherein, is the adjusted step size, is the unadjusted step size, k is a step size adjustment factor, when the environmental complexity is high, k is a step size reduction factor and when the environmental complexity is low, k is a step size increase factor and ; Step length adjustment strategy based on obstacle distance: Dynamically adjust the step length according to the distance between the robot and the nearest obstacle. The step length increases as the distance between the robot and the nearest obstacle increases, and decreases as the distance between the robot and the nearest obstacle decreases. The adjustment formula is: ； wherein, is a base step size, is a step size adjustment magnitude, is a reference distance, is a distance of the robot to the nearest obstacle; Step length adjustment strategy based on path curvature: In the curved path segment, adjust the step length according to the curvature radius of the current path. The adjustment formula is: ； wherein, is the maximum allowed step size for the robot in the current environment, is the radius of curvature of the current path, is the minimum radius of curvature that the robot can safely pass through, if the calculated then the adjusted step size is ; S4, obstacle avoidance control: Control the movement of the robot according to the planned path, adjust the speed and steering angle of the robot, and make the robot travel along the planned path. According to the real-time feedback of the environmental information, the movement of the robot is fine-tuned. S5, post-processing: Optimize and evaluate the path planning results and obstacle avoidance control process, smooth the planned path, evaluate the safety and feasibility of the path, and record the travel trajectory and obstacle avoidance information of the robot. S6, result output: Output the final planned path and obstacle avoidance control information to the execution system of the robot, and display the results in a visual way on the monitoring screen.

5. The method of claim 4, wherein, The specific steps of path planning in S3 are as follows: S3.1 initialization: Set the initial position, target position and working environment range of the robot, initialize the parameters of the improved dynamic RRT algorithm, including setting the initial position as the root node of the tree, the initial value of the exploration step length, the initial value of the expansion times, the step length adjustment coefficient based on environmental complexity, and the dynamic re-planning trigger threshold; S3.2 build RRT tree: From the root node, randomly generate a target point; Evaluate the current environmental complexity by calculating the number of obstacles per unit volume, adjust the exploration step length according to the environmental complexity, find the node closest to the target point in the tree, and grow a new node from the node to the target point in the direction of the target point with the adjusted step length. Repeat this process until the preset expansion times are reached or the nodes of the tree approach the target position. Record the information of each expansion node and its parent node during the expansion process; S3.3 path extraction: When the nodes of the tree approach the target position, extract the preliminary path from the nodes near the target position to the root node; Optimize the preliminary path, which includes removing redundant nodes and smoothing the path using a smoothing algorithm to form a smooth path; S3.4 real-time obstacle detection and dynamic re-planning: During the robot's travel along the planned path, the environmental perception module obtains real-time environmental information. If the distance between the detected obstacle and the robot is less than the dynamic re-planning trigger threshold, update the detected obstacle information to the environmental model, set the current position of the robot as the new starting position, keep the target position unchanged, and execute the above path planning process again to generate a new obstacle avoidance path.

6. The method of claim 4, wherein: The specific steps of obstacle avoidance control in S4 are as follows: S4.1 deviation calculation: real-time acquisition of the lateral deviation and the deviation change rate of the robot from the planned path, wherein the lateral deviation is the vertical distance from the actual position of the robot to the planned path, and the deviation change rate is the change amount of the lateral deviation per unit time; S4.2 control amount calculation: using a PID control algorithm, the output of the PID controller is calculated based on the lateral deviation and the deviation change rate: the calculation formula is: ； wherein u is an output of the PID controller, is a proportional coefficient, is an integral coefficient, is a differential coefficient, e is a lateral deviation, ∫edt is an integral term of the lateral deviation, and de / dt is a rate of change of the lateral deviation; S4.3 steering angle adjustment: according to the output of the PID controller and the gain of the robot steering system, the steering angle adjustment value is determined, so that the robot turns to the inside or outside of the path to correct the deviation and return to the planned path; S4.4 speed adjustment: the driving speed is dynamically adjusted in combination with the curvature radius of the planned path and the kinematic model of the robot, the smaller the curvature radius of the path, the lower the driving speed, the larger the curvature radius, the higher the driving speed, and the speed adjustment does not exceed the maximum allowable speed of the robot, and the kinematic model of the robot is represented as: ； wherein, is the adjusted speed, is the maximum speed of the robot, is the minimum radius of curvature that the robot can safely pass at the maximum speed.

7. The method of claim 4, wherein: In S3.3 and S5, a curve fitting algorithm is used for smoothing the path, and the curve fitting algorithm includes a Bezier curve algorithm.

Images