A special special operation equipment operation autonomous path planning and adaptive control method

Abstract

The present application relates to the field of intelligent control technology of special engineering machinery, in particular to a kind of special special operation equipment operation autonomous path planning and self-adapting control method, comprising five processes of scene three-dimensional reconstruction and constraint mapping, global initial path planning, dynamic obstacle prediction and local pre-correction, operation real-time self-adapting control, subsection closed loop iteration optimization are sequentially executed;Weighted total cost mathematical model is constructed including path length, terrain traffic, operation efficiency, collision time margin is introduced to quantify dynamic collision risk, incremental weight iteration formula is designed to realize operation parameter self-adapting adjustment, and multiple iteration trigger conditions and quantitative termination standards are set.The present application can quantitatively evaluate the influence of terrain and dynamic obstacle, dynamically adjust equipment operating parameters according to real-time operation deviation, continuously iteratively optimize path constraint weight online, effectively adapt to multiple complex limited operation scenarios such as curtain wall cleaning, tunnel detection and mountain inspection.

Description

Technical Field

[0001] This invention relates to the field of intelligent control technology for special engineering machinery, specifically to a method for autonomous path planning and adaptive control of special-purpose equipment. Background Technology

[0002] Various specialized special-purpose equipment are widely used in complex and constrained working conditions such as curtain wall cleaning, tunnel defect detection, and mountain line inspection. Traditional operation control methods heavily rely on manual operation. Although automated path planning technology has been initially applied, existing solutions generally have design shortcomings. Conventional path planning relies solely on a single spatial coordinate to construct an environmental model, uses path length as the optimization basis, and sets fixed control parameters to complete equipment movement and operation adjustments. It fails to simultaneously consider multiple constraints such as terrain conditions, equipment movement limits, operational accuracy, and dynamic obstacle evolution. At the same time, it lacks a complete link of hierarchical planning, real-time risk quantification, online adaptive correction, and segmented closed-loop optimization. In scenarios with undulating terrain, random obstacle movement, and stringent operational accuracy requirements, it is prone to problems such as path redundancy, delayed obstacle avoidance response, continuous deviations in operational trajectory, and equipment movement vibration, making it difficult to simultaneously meet the triple requirements of operational safety, operational efficiency, and operational accuracy.

[0003] Prior art document 1 (CN121625150A) discloses an automatic path generation scheme for aerial work platforms based on a BP neural network. This scheme relies on LiDAR to collect environmental information, rasterizes the work area, and extracts boom length and angle features as network input. Feasible paths are selected using an energy formula that sums two-dimensional distances. After the operation is completed, the neural network weights are updated offline using historical trajectories. When the obstacle distance is below a fixed threshold, a standardized obstacle avoidance maneuver of first retracting the boom and then adjusting the boom amplitude is executed. This scheme has significant technical limitations: raster modeling only focuses on the boom's own motion parameters, without incorporating environmental constraints such as slope, roughness, and load-bearing capacity; path evaluation simply calculates coordinate distances, without distinguishing the accessibility and operational costs of different work areas; it relies solely on fixed distance thresholds to determine collision risk, failing to predict obstacle movement trends or quantify the degree of collision hazard; the control model uses a fixed weight update logic, unable to dynamically adjust equipment movement and operation parameters based on real-time operational deviations; and it lacks a hierarchical architecture of global planning and local pre-correction, requiring a complete recalculation of the global path in the event of sudden obstacles, resulting in poor real-time control performance.

[0004] Comparative document 2 (CN119863755A) discloses a method and system for path planning of manual boom operations on truck-mounted cranes in confined spaces. It employs a fusion of binocular vision and LiDAR to construct a static 3D map, uses a convolutional neural network to identify rigid and deformable obstacles and set differentiated safety distances, generates an initial path through bidirectional probability sampling, filters the trajectory using load dynamics constraints, and completes smooth path adjustment using a virtual spring damping model. However, this solution has limited applicability to specific scenarios, a narrow technical scope, and several key technological deficiencies: environmental modeling only generates a static map, failing to construct a real-time updated planning space that integrates multi-dimensional operational constraints; the cost function is designed only for confined lifting conditions and cannot adapt to differentiated special operations such as those in curtain walls, mountains, and tunnels; it lacks dynamic obstacle trajectory prediction and collision margin quantification calculation processes, relying solely on static classification to set safety distances, with no early warning mechanism for approaching moving obstacles; trajectory adjustment relies on fixed damping parameters, lacking an incremental adaptive weight update algorithm based on cycle-by-cycle operational deviations; and it lacks a segmented closed-loop iterative optimization mechanism, failing to utilize deviation data from completed work segments to continuously correct subsequent paths and control parameters.

[0005] Based on a comprehensive review of conventional solutions and two comparative documents, current special equipment path control technologies exhibit six common shortcomings: First, insufficient environmental modeling dimensions, failing to integrate terrain, equipment, and operational constraints into a composite constraint space; second, simplistic path evaluation systems lacking weighted comprehensive cost calculation logic for path length, terrain accessibility, and operational efficiency; third, outdated dynamic obstacle handling methods, lacking trajectory prediction and quantitative risk assessment, relying solely on static distance-triggered obstacle avoidance; fourth, rigid control mechanisms, unable to adaptively adjust equipment operating parameters to follow real-time operational deviations; fifth, a lack of hierarchical planning architecture, resulting in high computational overhead and insufficient real-time performance for global replanning; and sixth, the absence of an online closed-loop iterative link, preventing segmented optimization of system parameters based on operational quality feedback. To address all these technical deficiencies, this invention proposes an autonomous path planning and adaptive control method for specialized special operations equipment. It designs multiple sets of innovative quantitative calculation formulas, constructing a complete five-layer process: scene mapping, global planning, local pre-correction, adaptive control, and closed-loop iteration. This fills the technical gaps in existing technologies and the two comparative documents, enabling autonomous, safe, and high-precision continuous operation in various complex special operations scenarios. Summary of the Invention

[0006] The purpose of this invention is to provide an autonomous path planning and adaptive control method for special-purpose operation equipment, in order to solve the problems mentioned in the background art, such as single constraint modeling of path planning for special-purpose operation equipment, one-sided path evaluation dimensions, inability to quantify and predict dynamic obstacle risks, fixed operation control parameters without adaptive correction, and lack of segmented online closed-loop iterative optimization mechanism, which lead to path redundancy, obstacle avoidance lag, operation trajectory deviation, and poor scene adaptability under complex working conditions.

[0007] To achieve the above objectives, the present invention provides the following technical solution: An autonomous path planning and adaptive control method for special-purpose equipment, comprising the following steps: S1 Operation Scene 3D Reconstruction and Constraint Mapping: By collecting environmental point cloud data, geometric parameters of the operation target, and physical properties of the operation surface through the multi-source sensing unit on the equipment, a 3D operation scene model is constructed, including the distribution of static obstacles, the access level of the operation surface, and the prohibited operation area. The motion performance constraints of the equipment body, the accuracy constraints of the operation execution end, and the safe operation distance threshold are mapped to the 3D operation scene model to generate a path planning constraint space with multi-dimensional constraints. S2 Global Initial Operation Path Planning: Within the path planning constraint space, with the goal of minimizing the overall cost of the entire operation process, an improved path search algorithm that integrates operation efficiency constraints is adopted to generate a global initial operation path covering all operation objectives, and to determine the equipment's travel node sequence, basic travel speed, and initial attitude parameters of the operation end. S3 Dynamic Obstacle Prediction and Local Path Pre-correction: Real-time acquisition of the position and motion data of dynamic obstacles in the work area, prediction of the future trajectory and spatiotemporal occupancy range of dynamic obstacles based on interactive multi-model filtering algorithm, calculation of collision risk level in combination with the equipment's path segment to be traveled, local pre-correction of the corresponding segment of the global initial work path based on the risk level, and generation of the work path to be executed. S4 Adaptive Control During Operation: During the operation of the equipment along the path to be executed, the actual operation deviation of the operation execution end and the six-axis attitude data of the equipment body are collected in real time. The equipment's travel speed, turning angle and position parameters of the operation execution end are dynamically adjusted through the adaptive control model to keep the actual operation deviation within the set accuracy threshold range. S5 Closed-Loop Iterative Optimization: Based on the work quality feedback data of the completed work segments, update the constraint weight coefficients and the correction coefficients of the adaptive control model in the path planning constraint space, and iteratively correct the path planning results and control parameters of the subsequent unworked segments until all work tasks are completed.

[0008] Preferably, the comprehensive cost of the entire operation process in step S2 is calculated using a weighted summation model, and the formula for calculating the total cost is: ; In the formula, This represents the total cost of a single path. For path length cost, The cost of terrain access, For the sake of operational efficiency, , , These are the weight coefficients of the corresponding cost terms, and they satisfy... .

[0009] Preferably, the terrain access cost The calculation is obtained by weighting the cost of slope, surface roughness, and bearing capacity of the working surface. The calculation formula is as follows: ; in , , The sub-item weight coefficients and satisfying Slope cost The slope cost is calculated by the angle between the point cloud normal vector at the path node in the 3D work scene model and the vertical direction; the larger the angle, the higher the slope cost; surface roughness cost. The elevation variance is calculated from the point cloud within a defined radius around the path node. A larger elevation variance indicates a more rugged surface, corresponding to a higher roughness cost; bearing capacity cost. The material type of the work surface is identified through binocular visual semantic segmentation and quantified by matching a preset material load-bearing capacity level table; during calculation, the three types of sub-costs corresponding to each path node are normalized to the [0,1] interval, with larger values ​​indicating higher accessibility of the corresponding work surface; the path length cost The operational efficiency cost is obtained by normalizing the ratio of the total length of the planned path to the length of the shortest feasible path within the work area. It is calculated by weighting the total number of attitude adjustments at the work end along the path with the difficulty coefficient of the work surface.

[0010] As a preferred option, when calculating the collision risk level in step S3, the collision time margin index is used for quantitative assessment, and the calculation formula is as follows: ; In the formula, For collision time margin, To measure the real-time relative distance between the equipment and dynamic obstacles, To set a safe distance threshold, These are the velocity components of the two objects along their relative motion direction. Correction coefficients for the work scenario.

[0011] As a preferred option, based on collision time margin The numerical values ​​categorize collision risks into three levels: low, medium, and high, with a pre-set first margin threshold. With the second margin threshold And satisfy The real-time relative distance The relative velocity component is obtained by real-time acquisition of the minimum distance between the equipment body and dynamic obstacles by lidar. The obstacle's velocity vector, output by the interactive multi-model filtering algorithm, is subtracted from the equipment's own travel velocity vector, and the component along the line connecting the two is extracted to obtain the safe distance threshold. Based on the equipment's external dimensions and operational safety specifications, the scenario correction coefficient... The value is set according to the terrain undulation and environmental visibility of the work scene, with a range of 0.8~1.2; when When the risk level is determined to be low, the original path and operational parameters are maintained, and the movement of obstacles is continuously tracked only according to the perception cycle; when When the risk level is determined to be medium, the equipment's travel speed is reduced proportionally according to the margin value, while the steering angle is slightly adjusted to increase the lateral distance from the obstacle, and the working end maintains its original working posture; when If the risk level is determined to be high, the current feed action is immediately suspended, triggering a local path replanning process; the risk level is recalculated and updated every complete sensing cycle.

[0012] As a preferred approach, the local path replanning under high-risk levels adopts an improved dynamic window method. Based on the two-dimensional sampling of linear and angular velocities in the traditional velocity space, it introduces positional deviation constraints of the operation execution end and tilt angle constraints of the equipment body. It limits the maximum offset of the operation end corresponding to each velocity combination within the sampling window to not exceed a set operation accuracy threshold and the tilt angle of the equipment to not exceed a preset safety threshold. An evaluation function is constructed that includes three items: obstacle avoidance distance score, global path fit score, and operation deviation score. The simulated trajectory corresponding to each set of sampled velocities is weighted and scored, and the trajectory with the highest comprehensive score is selected as the initial local obstacle avoidance path. Before outputting the final local path, a cubic B-spline curve is used to smooth the path node sequence to eliminate abrupt changes in angular velocity and pose jumps at the path inflection points.

[0013] Preferably, the adaptive control model in step S4 adopts an incremental weight iterative update mechanism, and the formula for updating the weight coefficients of the control parameters is: ; In the formula, Let k be the weighting coefficient of the k-th control parameter within the t-th control cycle. Let be the actual operational deviation during the t-th control cycle. The set standard operating deviation threshold, This represents the iteration step size coefficient.

[0014] Preferably, the k-th control parameter includes weights corresponding to three types of control quantities: travel speed, steering angle, and working end pose. The corresponding weight at the beginning of the t-th control cycle is obtained by iterative updates from the previous control cycle; the actual operational deviation The standard work deviation threshold is the lateral offset between the actual work trajectory acquired by the binocular vision camera and the preset work trajectory during the t-th control cycle. The iteration step size coefficient is preset according to the accuracy requirements of the task. The response bandwidth of the equipment actuator is set, with a value range of 0.01 to 0.1. The adaptive control model performs linkage control on the travel speed, turning angle, and six-degree-of-freedom pose of the working end. When the actual operation deviation increases and exceeds 50% of the accuracy threshold, the travel speed is reduced cycle by cycle according to a preset ratio, while the working end is driven to perform pose compensation in the opposite direction of the deviation. The compensation amount is directly proportional to the deviation amount. When the actual operation deviation is stable within the accuracy threshold range for three consecutive control cycles, the travel speed is gradually increased by a fixed step size until it is restored to the reference speed set by the global initial path. During the linkage control process, the operation accuracy is the first priority. When there is an execution conflict between speed adjustment and pose adjustment, the execution of the pose compensation action of the working end is given priority.

[0015] Preferably, the multi-source sensing unit in step S1 includes a lidar, a binocular vision camera, and an inertial measurement unit. During data fusion, time registration is performed first, using the hardware trigger clock of the lidar as a reference, and linear interpolation is used to align the binocular vision image frames and the high-frequency attitude data of the inertial measurement unit to the same time. Then, spatial registration is performed, using the pre-calibrated extrinsic parameter matrix between each sensor to transform the visual pixel coordinate system and the body coordinate system of the inertial measurement unit to the lidar coordinate system. The global pose of the equipment output by the synchronous positioning and mapping algorithm is combined to complete the stitching of the three-dimensional scene point cloud. Statistical filtering is used to remove discrete outliers, and radius filtering is used to remove small-scale noise points, finally generating a three-dimensional work scene model with adjustable grid resolution. The geometric parameters of the work target and the physical properties of the work surface are extracted from the registered visual image and point cloud data through a semantic segmentation network.

[0016] Preferably, the triggering conditions for the closed-loop iterative optimization in step S5 include three categories: First, the actual operation deviation of the completed work segment exceeds the set accuracy threshold for three consecutive control cycles; second, a new static obstacle or prohibited work area is detected in the work scenario, and the new constraint has spatial overlap with the work path to be executed; third, periodic optimization and update are triggered after each preset number of work node segments are completed. During iterative optimization, the constraint weight coefficients in the path planning constraint space and the correction coefficients of the adaptive control model are adjusted proportionally according to the deviation feedback data of the completed work segments, and all coefficient adjustments are set with upper and lower boundaries to avoid parameter divergence. The iteration termination criterion is that the fluctuation range of the total path cost of the subsequent unworked section is less than 5% for two consecutive iterations, and the predicted operation deviation meets the preset accuracy requirements. After the termination condition is met, the iteration stops, and the subsequent work tasks are executed according to the optimized path parameters and control parameters.

[0017] Compared with the prior art, the beneficial effects of the present invention are: 1. This invention constructs a multi-dimensional weighted cost model that integrates path length, terrain accessibility, and operational efficiency, along with a hierarchical planning architecture. First, it generates a constraint space by mapping a 3D scene with multi-dimensional constraints based on multi-source perception. Then, it performs global initial path optimization and combines dynamic obstacle motion prediction to achieve local path pre-correction. This invention can quantify the accessibility and operational costs of different work areas, selecting the optimal global path that balances accessibility and operational quality. Simultaneously, it quantifies collision risk levels through collision time margin, predicts the spatiotemporal occupancy range of dynamic obstacles in advance, avoids the lag problem caused by relying solely on fixed distance thresholds to trigger obstacle avoidance, reduces the computational overhead of path replanning, and improves the real-time performance and feasibility of path planning in complex dynamic scenarios.

[0018] 2. This invention can adaptively update the control weights according to the difference between the deviations of previous and subsequent operations on a cycle-by-cycle basis, quickly reduce the trajectory offset, and stably constrain the operation deviation within a preset accuracy threshold; at the same time, it distinguishes between high, medium and low collision risks and matches differentiated control strategies to achieve coordinated linkage between obstacle avoidance actions and operation actions, thus solving the technical defects of traditional control methods such as fixed parameters and slow convergence of operation deviations.

[0019] 3. This invention sets up multiple iterative trigger conditions and quantified termination criteria, and synchronously updates path constraint weights and control model coefficients based on segmented operation quality feedback to form a complete online closed-loop optimization link. This invention can perform segmented iterative optimization during operation execution, continuously adapting to real-time changes in terrain and obstacle conditions; the multi-source perception fusion standardized data registration and denoising process is compatible with various special equipment operation scenarios such as curtain wall cleaning, tunnel inspection, and mountain inspection, without the need to reconstruct algorithm logic for a single working condition, greatly reducing the cost of algorithm porting and modification, and forming a complete autonomous operation control logic from modeling, planning, control, and iteration, improving the versatility of the solution and long-term operational stability. Attached Figure Description

[0020] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are explained in detail together with the embodiments of the invention, but do not constitute a limitation thereof.

[0021] Figure 1 This is a flowchart illustrating the autonomous path planning and adaptive control method for special-purpose equipment according to the present invention. Figure 2 This is a flowchart of the three-dimensional reconstruction and constraint mapping process of the S1 operation scene in this invention; Figure 3 This is the global path planning (S2) and dynamic obstacle prediction (S3) diagram of the present invention. Figure 4 This is a block diagram of the high-risk local replanning and S4 adaptive control of the present invention; Figure 5 This is a block diagram of global path planning (S2) and dynamic obstacle prediction (S3) of the present invention. Detailed Implementation

[0022] The technical solutions of the present invention will be clearly and completely described below with reference to the embodiments of the present invention. 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.

[0023] like Figures 1-5 As shown, this invention is applied to specialized special-purpose equipment. The equipment includes a mobile carrier platform, a multi-source sensing unit, a work execution end, a main controller, and a motion execution mechanism. The multi-source sensing unit is mounted on the top of the mobile carrier platform, the work execution end is installed on the working side of the carrier platform, and the main controller is electrically connected to the sensing unit, the motion execution mechanism, and the work execution end, respectively, for executing a full-process method of autonomous path planning and adaptive control.

[0024] S1 Operation Scene 3D Reconstruction and Constraint Mapping: The first step is multi-source sensing data acquisition and spatiotemporal registration: The multi-source sensing unit includes a LiDAR, a binocular vision camera, and an inertial measurement unit (IMU). The LiDAR is used to acquire 3D point cloud data of the work area, with a horizontal scanning angle covering 360 degrees and a vertical scanning angle covering ±15 degrees, and a point cloud sampling frequency of 10Hz. The binocular vision camera is used to acquire visible light images and depth information of the work area, with an image resolution of 1920×1080 and a sampling frequency of 20Hz. The IMU is used to acquire six-axis attitude and acceleration data of the equipment, with a sampling frequency of 100Hz.

[0025] Data fusion begins with time registration. Using the LiDAR's hardware trigger clock as a unified time reference, linear interpolation is performed on the binocular vision image frames and the attitude data from the inertial measurement unit (IMU) to align the timestamps of the three types of data to the same moment, with the time registration error controlled within 1ms. Next, spatial registration is performed. The Zhang calibration method is used to pre-calibrate the extrinsic parameter matrices between each sensor, transforming the pixel coordinate system of the binocular vision and the body coordinate system of the IMU to the LiDAR coordinate system, thus unifying the spatial coordinates.

[0026] The second step is the construction of a 3D operational scene model: Combining the global pose of the equipment output by the laser-synchronized positioning and mapping algorithm, the registered point cloud data is stitched frame by frame to generate an initial point cloud map of the operational area. Point cloud denoising is then performed using statistical filtering and radius filtering. Statistical filtering removes outliers whose average distance from their neighbors exceeds three standard deviations. Radius filtering uses a spherical region with a radius of 0.1m centered on each point, retaining at least five valid points within the region. After denoising, the point cloud map is divided into a 0.2m × 0.2m × 0.2m 3D grid to generate a 3D operational scene model.

[0027] By using the improved PointNet++ semantic segmentation network, joint semantic recognition is performed on the registered point cloud and visual image to extract the outline of static obstacles, geometric parameters of the work target, material type and elevation information of the work surface, and to mark the prohibited work area and the access level of the work surface in the 3D work scene model.

[0028] The third step is multi-dimensional constraint mapping: The motion performance constraints of the equipment itself, the accuracy constraints of the operation execution end, and the safe operating distance threshold are mapped to the corresponding grids in the 3D operation scene model, generating a path planning constraint space. Motion performance constraints include a maximum climbing angle of 30 degrees, a minimum turning radius of 0.8m, and a maximum travel speed of 1.5m / s. Operation accuracy constraints include a positioning error at the operation end not exceeding ±5mm. The safe operating distance threshold is a minimum distance of 0.3m between the outer edge of the equipment and obstacles. During constraint mapping, grids exceeding the motion performance constraints are marked as impassable grids, and grids failing to meet the accuracy constraints are marked as low-operation-quality grids, which are then avoided or penalized in path planning.

[0029] S2 Global Initial Job Path Planning: Within the path planning constraint space, an improved A* algorithm that integrates job efficiency constraints is used for global path planning. The optimization objective is to minimize the overall cost of the entire job process, and to search for the optimal path that covers all job objectives.

[0030] The overall cost of the entire operation is calculated using a weighted summation model. The formula for calculating the total cost is as follows: ; In the formula, This represents the total cost of a single path, ranging from 0 to 1. , , These are the weighting coefficients for path length cost, terrain access cost, and operational efficiency cost, respectively. The sum of the three is 1, and they can be adjusted according to the priority of the task. The default values ​​are 0.3, 0.4, and 0.3, respectively.

[0031] Path length cost It is obtained by normalizing the ratio of the cumulative length of the planned path to the length of the shortest feasible path between two points within the work area. The calculation formula is as follows: ; In the formula, This represents the total length of the currently planned path. This represents the length of the feasible path corresponding to the shortest Euclidean distance from the starting point to the ending point.

[0032] Terrain access cost The calculation is obtained by weighting the cost of slope, surface roughness, and bearing capacity of the working surface. The calculation formula is as follows: ; In the formula, , , These are the weighting coefficients for the three types of sub-costs, with the sum of the three being 1. The default values ​​are 0.4, 0.3, and 0.3, respectively.

[0033] Slope cost The angle between the point cloud normal vector at the path node in the 3D operation scene model and the vertical direction is calculated. The angle ranges from 0 degrees to 90 degrees. The angle value is normalized to the range of 0 to 1 by dividing by 90 degrees. The larger the angle, the higher the slope cost.

[0034] Surface roughness cost The elevation variance of all point clouds within a 0.5m radius around the path node is calculated. The variance value is then normalized to the range of 0 to 1 by dividing it by the maximum elevation variance of the area. The larger the variance, the more rugged the surface, and the higher the corresponding roughness cost.

[0035] Bearing capacity cost The material type of the work surface is identified by semantic segmentation and quantified by matching it with a preset material bearing capacity level table. The material types include concrete, asphalt, soil, crushed stone, etc. The corresponding bearing capacity level is assigned from high to low and normalized to the range of 0 to 1. The weaker the bearing capacity, the higher the cost.

[0036] Operational efficiency cost The result is calculated by weighting the total number of attitude adjustments at the work site along the path with the difficulty coefficient of the work surface. The calculation formula is as follows: ; In the formula, This represents the cumulative number of attitude adjustments made by the workpiece along the path. This is the preset maximum number of adjustments allowed for a single path segment. The difficulty coefficient of the working surface ranges from 0.8 to 1.2 and is determined by a combination of the surface roughness and slope. After the global path planning is completed, the sequence of equipment travel nodes, the basic travel speed of each node, and the initial attitude parameters of the working end are output as the global initial working path.

[0037] S3 Dynamic Obstacle Prediction and Local Path Pre-correction: The first step is dynamic obstacle trajectory prediction: During equipment operation, LiDAR and binocular vision cameras collect the position, size, and motion data of dynamic obstacles within the operation area at a fixed perception cycle. An interactive multi-model filtering algorithm is then used to estimate and predict the motion state of the dynamic obstacles. The interactive multi-model filtering includes two sub-models: a uniform motion model and a uniformly accelerated motion model. The probability weights of the two models are updated in real time according to the obstacle's motion state. The prediction duration is set to 3 seconds, with a time step of 0.1 seconds, outputting the obstacle's motion trajectory and spatiotemporal occupancy range within the next 3 seconds.

[0038] The second step is collision risk level assessment: combining the equipment's planned path segment with the predicted obstacle trajectory, the collision time margin is calculated to quantify the collision risk level. The calculation formula is as follows: ; In the formula, This represents the collision time margin, expressed in seconds. The real-time minimum relative distance between the equipment body and dynamic obstacles is obtained by real-time ranging using lidar. The preset safe distance threshold is set to 0.5m based on the equipment's dimensions and operating procedures. The relative velocity component between the equipment and the obstacle along the line connecting them is obtained by subtracting the equipment's own velocity vector from the obstacle's predicted velocity vector and then projecting it onto the line connecting them. This is a correction factor for the work scene, with a value ranging from 0.8 to 1.2. For scenes with large terrain undulations and low visibility, a value greater than 1 is used, while for flat and open scenes, a value less than 1 is used.

[0039] Collision risk is divided into three levels based on the collision time margin value, and a first margin threshold is preset. The second margin threshold is 2 seconds. It takes 0.8 seconds. If the risk level is determined to be low, maintain the original path and operational parameters, and continuously track the obstacle's movement status only according to the perception cycle. If the risk level is determined to be medium, the equipment's travel speed is reduced proportionally according to the margin value, while the steering angle is slightly adjusted to increase the lateral distance from obstacles, and the working end maintains its original working posture. If the risk level is determined to be high, the current feed action is immediately suspended, triggering a local path replanning process. The risk level is recalculated and updated every sensing cycle.

[0040] The third step, local path replanning and smoothing: Under high-risk conditions, an improved dynamic window method is used for local path replanning. Based on the traditional dynamic window method's two-dimensional sampling of linear and angular velocities, positional deviation constraints at the work execution end and tilt angle constraints on the equipment body are introduced. The maximum offset of the work end corresponding to each velocity combination within the sampling window is limited to not exceeding a set work accuracy threshold, and the equipment tilt angle is limited to not exceeding a preset safety threshold of 15 degrees. An evaluation function containing three indicators is constructed: ; In the formula, The obstacle avoidance distance score is positively correlated with the distance to the obstacle. The global path fit score is negatively correlated with the deviation between the local path and the global path. The score for operational deviation is negatively correlated with the deviation of the operational end position. , , The corresponding weighting coefficients have default values ​​of 0.4, 0.3, and 0.3.

[0041] For each set of sampling velocities, an evaluation function value is calculated for the simulated trajectory, and the trajectory with the highest comprehensive score is selected as the initial local obstacle avoidance path. Before outputting the final local path, a cubic B-spline curve is used to smoothly fit the path node sequence to eliminate sudden changes in angular velocity and pose jumps at path inflection points, generating the task path to be executed.

[0042] S4 Adaptive Control During Operation: During the operation of the equipment along the path to be executed, the actual operation deviation of the operation execution end and the six-axis attitude data of the equipment body are collected at a fixed control cycle. The equipment's travel speed, turning angle and position parameters of the operation execution end are dynamically adjusted through the adaptive control model.

[0043] The adaptive control model employs an incremental weight iterative update mechanism, and the update formula for the weight coefficients of the control parameters is as follows: ; In the formula, For the first Within the first regulation cycle The weighting coefficient of the first control parameter, the first The control parameters correspond to three types of control quantities: travel speed, steering angle, and working end posture. For the first The actual operational deviation for each control cycle is calculated from the lateral offset between the actual operational trajectory captured by the binocular vision camera and the preset operational trajectory. This represents the actual operational deviation from the previous control cycle. The standard operating deviation threshold is set to 5mm based on the required operating accuracy. The iteration step size coefficient is set according to the response bandwidth of the equipment actuator, and its value ranges from 0.01 to 0.1, with a default value of 0.05.

[0044] The adaptive control model performs coordinated control of three types of control parameters. When the actual operational deviation increases and exceeds 50% of the accuracy threshold, the travel speed is reduced cycle by cycle according to the weighted coefficient ratio. Simultaneously, the work execution end is driven to perform pose compensation in the opposite direction of the deviation, with the compensation amount being directly proportional to the deviation amount. When the actual operational deviation stabilizes within the accuracy threshold range for three consecutive control cycles, the travel speed is gradually increased by a fixed step size until it returns to the reference speed set by the global initial path. During the coordinated control process, operational accuracy is the first priority. When there is an execution conflict between speed adjustment and pose adjustment, the execution of pose compensation actions at the work end is prioritized.

[0045] S5 Closed-Loop Iterative Optimization: Based on the feedback data of completed work segments' work quality, closed-loop iterative optimization is performed on path planning and control parameters. The triggering conditions for closed-loop iterative optimization include three categories: First, the actual work deviation of the completed work segment exceeds the set accuracy threshold for three consecutive control cycles. Second, a new static obstacle or prohibited work area is detected in the work scenario, and this new constraint spatially overlaps with the work path to be executed. Third, periodic optimization updates are triggered after every 20 work nodes are completed.

[0046] During iterative optimization, the constraint weight coefficients within the path planning constraint space and the correction coefficients of the adaptive control model are adjusted proportionally based on the deviation feedback data of the completed segments. All coefficient adjustments have upper and lower boundaries set; the upper and lower boundaries of the weight coefficients are 0.5 to 1.5 times the initial value to prevent parameter divergence that could lead to system instability.

[0047] The iteration termination criterion is that the fluctuation range of the total path cost of the subsequent unworked sections is less than 5% for two consecutive iterations, and the predicted operation deviation meets the preset accuracy requirements. Once the termination condition is met, iteration stops, and subsequent operation tasks are executed according to the optimized path parameters and control parameters until all operation objectives are completed.

[0048] Example 1: Application of Specialized Equipment for Building Curtain Wall Cleaning This embodiment uses special equipment for high-altitude curtain wall cleaning. The operation scenario is the glass curtain wall facade of an office building with a height of 80m and a width of 30m. There are a few decorative protrusions on the curtain wall surface as static obstacles, and there are falling objects and maintenance baskets as dynamic obstacles during the operation.

[0049] The equipment is equipped with a 16-line lidar, a binocular industrial camera, and a six-axis inertial measurement unit. The working end is a rotating cleaning disc, and the working accuracy requirement is that the cleaning trajectory deviation does not exceed 10mm.

[0050] The parameter is set as a weighting coefficient. , , Safe distance threshold m. Scene correction factor Standard operating deviation threshold mm. Iteration step size coefficient. .

[0051] Path planning calculation example: Select the shortest feasible path length from the start point of the task to the first task segment. m, planned path length m, calculate the path length cost After normalization, it becomes 0.73.

[0052] Average slope cost of the path nodes Roughness cost Bearing capacity cost ,Pick , , Calculated Number of attitude adjustments for this path segment Maximum number of adjustments allowed Next, the difficulty level of the work surface Calculated Final total cost .

[0053] Obstacle avoidance calculation example: The relative distance between the dynamic gondola and the equipment is detected. m, relative velocity m / s, substituting into the collision time margin formula, we get If the value of s is greater than the first threshold of 2s, it is determined to be low risk, and the original path is maintained.

[0054] Example of regulation calculation: Periodic operation deviation mm, no. Periodic deviation mm, initial value of travel speed weight Substituting into the weight update formula, we get The weighting increased slightly, and the corresponding speed was slightly reduced.

[0055] Example 2: Application of Specialized Equipment for Tunnel Defect Detection This embodiment uses special equipment for detecting defects in the inner wall of tunnels. The operation scenario is a one-way two-lane highway tunnel with a length of 500m. There are maintenance passages and fire extinguisher boxes as static obstacles in the tunnel, and inspection vehicles as dynamic obstacles during the operation.

[0056] The equipment is equipped with a 32-line lidar, a binocular high-definition camera, and a high-precision inertial measurement unit. The operation execution end is a linear array scanning detection module, and the operation accuracy requirement is that the defect location deviation does not exceed 8mm.

[0057] The parameter is set as a weighting coefficient. , , Safe distance threshold m. Scene correction factor Standard operating deviation threshold mm. Iteration step size coefficient. .

[0058] Path planning calculation example: Select a 100m long detection section, find the shortest feasible path length. m, planned path length The normalized path length cost, calculated using the formula m, is 0.83. The average slope cost for each node along this path is... Roughness cost Bearing capacity cost Calculated .

[0059] Number of attitude adjustments for this path segment Maximum number of adjustments allowed Next, the difficulty level of the work surface Calculated Final total cost .

[0060] Obstacle avoidance calculation example: The relative distance between the inspection vehicle and the equipment is detected. m, relative velocity m / s, substituting into the formula, we get If the value of s is less than the second threshold of 0.8s, it is judged as high risk and triggers local path replanning.

[0061] Example of regulation calculation: Periodic operation deviation mm, no. Periodic deviation mm, initial values ​​of pose weights at the working end Substituting into the formula, we get The pose compensation weight is increased, which enhances the deviation correction.

[0062] Example 3: Application of Specialized Equipment for Inspecting Mountain Transmission Lines This embodiment uses special equipment for inspecting power transmission lines in mountainous areas. The operation scenario is a power transmission line corridor in a mountainous and hilly area. The operation section is 2km long. There are trees and rocks as static obstacles in the area, and birds and small wild animals as dynamic obstacles during the operation.

[0063] The equipment is equipped with a 16-line lidar, a binocular wide-angle camera, and a high vibration-resistant inertial measurement unit. The operation execution end is a visible light and infrared dual-light inspection gimbal. The operation accuracy requirement is that the line inspection deviation does not exceed 15mm.

[0064] The parameter is set as a weighting coefficient. , , Safe distance threshold m. Scene correction factor Standard operating deviation threshold mm. Iteration step size coefficient. .

[0065] Path planning calculation example: Select a 200m long inspection section, find the shortest feasible path length. m, planned path length The normalized path length cost, calculated using the formula m, is 0.91. The average slope cost for each node along this path is... Roughness cost Bearing capacity cost Calculated .

[0066] Number of attitude adjustments for this path segment Maximum number of adjustments allowed Next, the difficulty level of the work surface Calculated Final total cost .

[0067] Obstacle avoidance calculation example: Detecting the relative distance between a flying obstacle and the equipment. m, relative velocity m / s, substituting into the formula, we get If the speed is between 0.8s and 2s, it is considered a medium risk, so the speed is reduced and the steering angle is slightly adjusted.

[0068] Example of regulation calculation: Periodic operation deviation mm, no. Periodic deviation mm, initial value of steering angle weight Substituting into the formula, we get The weighting decreased slightly, corresponding to a weaker shift in the correction.

[0069] Performance data comparison of examples, test results data table: Work section size 2400㎡ working area 500m detection length 2000m inspection length Total cost of planning path 0.365 0.377 0.565 Average work deviation 4.2mm 3.7mm 8.5mm Dynamic obstacle avoidance success rate 98.2% 99.1% 96.5% Average obstacle avoidance response time 0.21s 0.18s 0.26s Work efficiency per unit time 120㎡ / h 420m / h 1.8km / h Number of closed-loop iterations 3 times 2 times 5 times The data in the table above shows that the method of this invention can stably achieve autonomous path planning and adaptive control in three different special operation scenarios. In the scenarios of curtain walls and tunnels with flat terrain and high operational precision requirements, the total path cost is at a low level, the average operational deviation is controlled within 5mm, and the obstacle avoidance success rate is higher than 98%, which can meet the index requirements of high-precision special operations. In the scenario of mountain inspection with complex terrain, the increased terrain passage cost leads to an increase in the total path cost, but the average operational deviation is still controlled within 10mm, and the obstacle avoidance success rate remains above 96%. At the same time, the parameters are continuously adjusted through 5 closed-loop iterations to ensure the stability of operations and the safety of passage in complex terrain. The average obstacle avoidance response time in all three scenarios is less than 0.3s, which can quickly respond to sudden dynamic obstacles, ensuring both operational precision and operational efficiency.

[0070] This invention constructs a multi-dimensional weighted cost model that integrates path length, terrain accessibility, and operational efficiency, along with a hierarchical planning architecture. First, it generates a constraint space by mapping a 3D scene with multi-dimensional constraints based on multi-source perception. Then, it performs global initial path optimization and combines dynamic obstacle motion prediction to achieve local path pre-correction. Compared to the comparative documents' technical solutions that rely solely on single coordinate distances to evaluate paths and lack hierarchical planning, this invention can quantify the accessibility and operational costs of different work areas, selecting the optimal global path that balances accessibility and operational quality. Simultaneously, it quantifies collision risk levels through collision time margin, predicting the spatiotemporal occupancy range of dynamic obstacles in advance, avoiding the lag problem caused by relying solely on fixed distance thresholds to trigger obstacle avoidance, reducing the computational overhead of path replanning, and improving the real-time performance and feasibility of path planning in complex dynamic scenarios.

[0071] This invention designs an incremental weighted iterative update control formula based on real-time operational deviations, establishing a linkage control logic for travel speed, steering angle, and work end pose, dynamically correcting execution parameters with operational accuracy as the priority. In contrast, the comparative paper uses a static control model with fixed weights and fixed damping parameters, which cannot dynamically adjust the control quantity according to real-time operational errors. This invention can adaptively update the control weights cycle by cycle based on the difference in operational deviations, quickly reducing trajectory offset and stably constraining operational deviations within a preset accuracy threshold. Simultaneously, it differentiates between high, medium, and low collision risks, matching different control strategies to achieve coordinated linkage between obstacle avoidance and operational actions, overcoming the technical shortcomings of traditional control methods such as fixed parameters and slow convergence of operational deviations.

[0072] This invention sets up multiple iterative triggering conditions and quantified termination criteria, and synchronously updates path constraint weights and control model coefficients based on segmented operation quality feedback to form a complete online closed-loop optimization link. In contrast, existing models can only be updated offline or lack iterative correction mechanisms. This invention can perform segmented iterative optimization during operation, continuously adapting to real-time changes in terrain and obstacle conditions. The standardized data registration and denoising process based on multi-source perception fusion is compatible with various special equipment operation scenarios, including curtain wall cleaning, tunnel inspection, and mountain patrol. It eliminates the need to reconstruct algorithm logic for single operating conditions, significantly reducing algorithm porting and modification costs. From modeling, planning, control, and iteration, it forms a complete autonomous operation control logic, improving the versatility and long-term operational stability of the solution.

[0073] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely preferred examples and are not intended to limit the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed. The scope of protection of the present invention is defined by the appended claims and their equivalents.

Claims

1. A method for autonomous path planning and adaptive control of special-purpose equipment, characterized in that, Includes the following steps: S1 Operation Scene 3D Reconstruction and Constraint Mapping: By collecting environmental point cloud data, geometric parameters of the operation target, and physical properties of the operation surface through the multi-source sensing unit on the equipment, a 3D operation scene model is constructed, including the distribution of static obstacles, the access level of the operation surface, and the prohibited operation area. The motion performance constraints of the equipment body, the accuracy constraints of the operation execution end, and the safe operation distance threshold are mapped to the 3D operation scene model to generate a path planning constraint space with multi-dimensional constraints. S2 Global Initial Operation Path Planning: Within the path planning constraint space, with the goal of minimizing the overall cost of the entire operation process, an improved path search algorithm that integrates operation efficiency constraints is adopted to generate a global initial operation path covering all operation objectives, and to determine the equipment's travel node sequence, basic travel speed, and initial attitude parameters of the operation end. S3 Dynamic Obstacle Prediction and Local Path Pre-correction: Real-time acquisition of the position and motion data of dynamic obstacles in the work area, prediction of the future trajectory and spatiotemporal occupancy range of dynamic obstacles based on interactive multi-model filtering algorithm, calculation of collision risk level in combination with the equipment's path segment to be traveled, local pre-correction of the corresponding segment of the global initial work path based on the risk level, and generation of the work path to be executed. S4 Adaptive Control During Operation: During the operation of the equipment along the path to be executed, the actual operation deviation of the operation execution end and the six-axis attitude data of the equipment body are collected in real time. The equipment's travel speed, turning angle and position parameters of the operation execution end are dynamically adjusted through the adaptive control model to keep the actual operation deviation within the set accuracy threshold range. S5 Closed-Loop Iterative Optimization: Based on the work quality feedback data of the completed work segments, update the constraint weight coefficients and the correction coefficients of the adaptive control model in the path planning constraint space, and iteratively correct the path planning results and control parameters of the subsequent unworked segments until all work tasks are completed.

2. The autonomous path planning and adaptive control method for special-purpose equipment operation according to claim 1, characterized in that, The total cost of the entire operation described in step S2 is calculated using a weighted summation model. The formula for calculating the total cost is as follows: ; In the formula, This represents the total cost of a single path. For path length cost, The cost of navigating the terrain, For the sake of operational efficiency, , , These are the weight coefficients of the corresponding cost terms, and they satisfy... .

3. The autonomous path planning and adaptive control method for special-purpose equipment operation according to claim 2, characterized in that, The terrain access cost The calculation is obtained by weighting the cost of slope, surface roughness, and bearing capacity of the working surface. The calculation formula is as follows: ； in , , The sub-item weight coefficients and satisfying Slope cost The slope cost is calculated by the angle between the point cloud normal vector at the path node in the 3D work scene model and the vertical direction; the larger the angle, the higher the slope cost; surface roughness cost. The elevation variance is calculated from the point cloud within a defined radius around the path nodes. A larger elevation variance indicates a more rugged surface, corresponding to a higher roughness cost; bearing capacity cost. The material type of the work surface is identified through binocular visual semantic segmentation and quantified by matching a preset material load-bearing capacity level table; during calculation, the three types of sub-costs corresponding to each path node are normalized to the [0,1] interval, with larger values ​​indicating higher accessibility of the corresponding work surface; the path length cost The operational efficiency cost is obtained by normalizing the ratio of the total length of the planned path to the length of the shortest feasible path within the work area. It is calculated by weighting the total number of attitude adjustments at the work end along the path with the difficulty coefficient of the work surface.

4. The autonomous path planning and adaptive control method for special-purpose equipment operation according to claim 1, characterized in that, In step S3, when calculating the collision risk level, the collision time margin index is used for quantitative assessment. The calculation formula is as follows: ; In the formula, For collision time margin, To measure the real-time relative distance between the equipment and dynamic obstacles, To set a safe distance threshold, These are the velocity components of the two objects along their relative motion direction. Correction coefficients for the work scenario.

5. The autonomous path planning and adaptive control method for special-purpose equipment operation according to claim 4, characterized in that, Based on collision time margin The numerical values ​​categorize collision risks into three levels: low, medium, and high, with a pre-set first margin threshold. With the second margin threshold And satisfy The real-time relative distance The relative velocity component is obtained by real-time acquisition of the minimum distance between the equipment body and dynamic obstacles by lidar. The obstacle's velocity vector, output by the interactive multi-model filtering algorithm, is subtracted from the equipment's own travel velocity vector, and the component along the line connecting the two is extracted to obtain the safe distance threshold. Based on the equipment's external dimensions and operational safety specifications, the scenario correction coefficient... The value is set according to the terrain undulation and environmental visibility of the work scene, with a range of 0.8 to 1.2; when If the risk level is determined to be low, the original path and operation parameters are maintained, and the movement status of the obstacle is continuously tracked only according to the perception cycle. when If the risk level is determined to be medium, the equipment speed will be reduced proportionally according to the margin value, while the steering angle will be slightly adjusted to increase the lateral distance from the obstacle, and the working end will maintain the original working posture. when If the risk level is determined to be high, the current feed action is immediately suspended, triggering a local path replanning process; the risk level is recalculated and updated every complete sensing cycle.

6. The autonomous path planning and adaptive control method for special-purpose equipment operation according to claim 5, characterized in that, For local path replanning under high-risk levels, an improved dynamic window method is adopted. Based on the two-dimensional sampling of linear and angular velocities in the traditional velocity space, positional deviation constraints of the operation execution end and tilt angle constraints of the equipment body are introduced. It is limited that the maximum offset of the operation end corresponding to each velocity combination within the sampling window does not exceed the set operation accuracy threshold, and the tilt angle of the equipment does not exceed the preset safety threshold. An evaluation function is constructed that includes three items: obstacle avoidance distance score, global path fit score, and operation deviation score. The simulated trajectory corresponding to each sampling velocity is weighted and scored, and the trajectory with the highest comprehensive score is selected as the initial local obstacle avoidance path. Before outputting the final local path, a cubic B-spline curve is used to smooth the path node sequence to eliminate abrupt changes in angular velocity and pose jumps at the path inflection points.

7. The autonomous path planning and adaptive control method for special-purpose equipment operation according to claim 1, characterized in that, The adaptive control model described in step S4 adopts an incremental weight iterative update mechanism, and the update formula for the weight coefficients of the control parameters is as follows: ; In the formula, Let k be the weighting coefficient of the k-th control parameter within the t-th control cycle. Let be the actual operational deviation during the t-th control cycle. The set standard operating deviation threshold, This represents the iteration step size coefficient.

8. The autonomous path planning and adaptive control method for special-purpose equipment operation according to claim 7, characterized in that, The k-th control parameter includes weights corresponding to three types of control quantities: travel speed, steering angle, and working end pose. The corresponding weight at the beginning of the t-th control cycle is obtained by iterative updates from the previous control cycle; the actual operational deviation The standard work deviation threshold is the lateral offset between the actual work trajectory acquired by the binocular vision camera and the preset work trajectory during the t-th control cycle. The iteration step size coefficient is preset according to the accuracy requirements of the task. The value is set according to the response bandwidth of the equipment actuator, and the range is 0.01~0.1; The adaptive control model coordinates the travel speed, steering angle, and six-degree-of-freedom pose of the workpiece. When the actual work deviation increases and exceeds 50% of the accuracy threshold, the travel speed is reduced cycle by cycle according to a preset ratio, while the workpiece is driven to perform pose compensation in the opposite direction of the deviation. The compensation amount is directly proportional to the deviation amount. When the actual work deviation is stable within the accuracy threshold range for three consecutive control cycles, the travel speed is gradually increased by a fixed step size until it is restored to the reference speed set by the global initial path. During the coordinated control process, work accuracy is given the first priority. When there is an execution conflict between speed adjustment and pose adjustment, the execution of pose compensation action of the workpiece is given priority.

9. The autonomous path planning and adaptive control method for special-purpose equipment operation according to claim 1, characterized in that, The multi-source sensing unit mentioned in step S1 includes a lidar, a binocular vision camera, and an inertial measurement unit. During data fusion, time registration is performed first. The hardware trigger clock of the lidar is used as a reference, and the binocular vision image frame and the high-frequency attitude data of the inertial measurement unit are aligned to the same time through linear interpolation. Then, spatial registration is performed. Using the pre-calibrated extrinsic parameter matrices between the sensors, the visual pixel coordinate system and the inertial measurement unit's body coordinate system are transformed into the lidar coordinate system. The global pose of the equipment output by the synchronous positioning and mapping algorithm is combined to complete the stitching of the 3D scene point cloud. Statistical filtering is used to remove discrete outliers, and radius filtering is used to remove small-scale noise points. Finally, a 3D operation scene model with adjustable grid resolution is generated. The geometric parameters of the operation target and the physical properties of the operation surface are extracted from the registered visual image and point cloud data through a semantic segmentation network.

10. The autonomous path planning and adaptive control method for special-purpose equipment operation according to claim 1, characterized in that, The triggering conditions for the closed-loop iterative optimization described in step S5 include three categories: First, the actual operation deviation of the completed work segment exceeds the set accuracy threshold for three consecutive control cycles; second, a new static obstacle or prohibited work area is detected in the work scenario, and the new constraint has spatial overlap with the work path to be executed; third, periodic optimization and update are triggered after each preset number of work node segments are completed. During iterative optimization, the constraint weight coefficients in the path planning constraint space and the correction coefficients of the adaptive control model are adjusted proportionally based on the deviation feedback data of the completed work segments, and all coefficient adjustments are set with upper and lower boundaries to avoid parameter divergence. The iteration termination criterion is that the fluctuation range of the total path cost of the subsequent unworked section is less than 5% for two consecutive iterations, and the predicted operation deviation meets the preset accuracy requirements. After the termination condition is met, the iteration stops, and the subsequent operation tasks are executed according to the optimized path parameters and control parameters.

Images