A crane dynamic path planning and obstacle avoidance method and system based on hybrid A* and TEB fusion
By integrating hybrid and TEB path planning methods, the challenges of path planning and obstacle avoidance for cranes in complex environments are solved, generating smooth paths, preventing mechanical wear and boom scraping, and improving the safety and efficiency of cranes.
Patent Information
- Application Number
- CN202610929406.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-06-25
- Publication Date
- 2026-11-06
AI Technical Summary
Existing path planning and obstacle avoidance algorithms fail to adequately consider the nonholonomic kinematic constraints of large machinery in crane applications, leading to mechanical wear and steering delays; they are unable to cope with complex dynamic environments, easily causing severe swaying of suspended objects and scraping of the boom; and they lack the ability to predict dynamic obstacles in the long term, making collisions more likely.
A path planning method based on hybrid and TEB fusion is adopted. By constructing a multi-layer cost map and combining the nonholonomic kinematic constraints of the crane, a smooth path is generated. Anti-sway and boom collision avoidance penalties are introduced. The time elastic band algorithm is used for local trajectory optimization, predicting dynamic obstacles and performing adaptive avoidance.
The generated path eliminates sharp angles, reduces mechanical wear and steering delay, prevents suspended objects from swaying and boom scraping, and improves the safety and efficiency of cranes in complex environments.
Smart Images
Figure CN123047546A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of automation and intelligent control technology for engineering machinery, specifically relating to a hybrid-based... A method and system for dynamic path planning and obstacle avoidance of cranes integrated with a time-flexible band. Background Technology
[0002] With the development of intelligent construction, cranes, as the core material handling equipment on construction sites, require a high level of autonomous navigation and intelligent operation. Real construction sites are often highly unstructured and dynamically complex environments with numerous static obstacles and unpredictable dynamic obstacles.
[0003] Currently, existing path planning and obstacle avoidance algorithms have the following significant technical shortcomings when applied to cranes, a special type of large-scale engineering machinery:
[0004] The nonholonomic constraints of large machinery are not adequately considered. Traditional global programming algorithms... The Dijkstra algorithm and similar algorithms often generate polylines with sharp angles. Cranes, constrained by minimum turning radii, cannot directly follow and execute these paths; forcing them would lead to frequent in-place turns and severe mechanical wear.
[0005] The ability to cope with dynamic environments involving both humans and machines is insufficient. Traditional local obstacle avoidance algorithms are prone to getting stuck in local optima in complex construction sites, causing cranes to "lock up" and stop. At the same time, existing algorithms lack the ability to predict the future trajectory of dynamic obstacles in the long term, and are prone to collisions due to insufficient braking distance in the face of sudden situations.
[0006] The dynamic characteristics of "flexible suspension" during loaded operations are ignored. Existing algorithms often treat the equipment as a rigid body, failing to strictly constrain the curvature and acceleration / deceleration of the running trajectory. When a crane makes sharp turns or accelerates abruptly, it is highly susceptible to violent swaying of the suspended load, leading to operational inaccuracies or even complete instability and overturning.
[0007] There is a lack of effective collision avoidance constraints for the three-dimensional extension of the boom. Traditional two-dimensional cost maps can only describe the projected outline of the crane chassis and cannot represent the tall and outwardly extending boom. This can easily lead to the boom scraping against high-altitude obstacles when there is no risk of collision with the crane chassis. Summary of the Invention
[0008] This invention addresses the shortcomings of existing technologies in simultaneously ensuring global path optimization while considering the nonholonomic constraints of large cranes, proactive obstacle avoidance in complex dynamic environments, and the failure to effectively solve the problems of load-bearing anti-sway and boom spatial collision avoidance. It provides a hybrid-based approach... The crane dynamic path planning and obstacle avoidance method integrated with TEB, through global hybridization The algorithm strips away simple grid search and deeply integrates the nonholonomic kinematic constraints of the crane using dual heuristic functions. The generated path eliminates sharp angles from the source, limits the anti-sway lateral acceleration as a cost constraint, and establishes an independent dynamic projection envelope for the boom. It introduces multimodal dynamic target prediction and adaptive spatiotemporal safety boundaries, no longer avoiding all dynamic obstacles in a one-size-fits-all manner. This significantly reduces the wear and steering delay of the underlying mechanical mechanism, eliminates the risk of high-altitude boom scraping in blind spots, and achieves three-dimensional active safety protection against swaying and boom scraping under load.
[0009] According to one aspect of the present invention, a hybrid-based Crane dynamic path planning and obstacle avoidance methods integrated with TEB include:
[0010] Step S1: Based on the real-time acquired environmental point cloud and image data of the construction site, construct a multi-layer cost map containing a static obstacle layer and a dynamic obstacle layer, and expand the static obstacle layer and the dynamic obstacle layer according to the physical outline size of the crane to generate a crane operation safety boundary layer.
[0011] Step S2: Based on the crane operation safety boundary layer, and combined with the nonholonomic kinematic constraints of the crane chassis, establish a kinematic model, and adopt a hybrid approach. The algorithm performs a global node expansion search in the continuous state space of the kinematic model to generate an initial global smooth path;
[0012] Step S3: Based on the initial global smooth path, an initial state set is obtained. Combined with the dynamic obstacle layer in the multi-layer cost map, the initial state set is subjected to local trajectory deformation and time parameter optimization under the constraint of a preset multi-objective cost function using the time elastic band algorithm to generate a local obstacle avoidance trajectory. The local obstacle avoidance trajectory is used to generate control commands to drive the crane.
[0013] As a further technical solution, the hybrid The algorithm uses a dual heuristic function, expressed as follows:
[0014]
[0015] in, It is a dual heuristic function. For the current search node, For the target node; To ignore environmental obstacles, the current node To the target node Reeds-Shepp curve distance; The subscript RS in the text indicates the Reeds-Shepp curve distance; To ignore the kinematic constraints of the crane, the current search node on the two-dimensional grid map To the target node The shortest collision-free path distance; The subscript 2D in the text indicates the path distance in a two-dimensional plane.
[0016] As a further technical solution, the expression for the preset multi-objective cost function is:
[0017]
[0018] in, For the preset multi-objective cost function, For trajectory following penalty items, The weighting coefficients for the trajectory following penalty term. This is a kinematic limit penalty term. The weighting coefficients for the kinematic limit penalty term. This is a standard obstacle avoidance penalty item. The weighting coefficients for the standard obstacle avoidance penalty items. For cranes, a load-bearing anti-sway penalty item is provided. The weighting coefficient for the crane's load-bearing anti-sway penalty term. For crane boom collision avoidance penalties, This is the weighting coefficient for the collision avoidance penalty item of the crane boom.
[0019] As a further technical solution, the constraint condition for the crane's load-bearing anti-sway penalty term is as follows:
[0020] Constraints are imposed on the linear velocity and trajectory curvature of the local obstacle avoidance trajectory so that the linear velocity of the crane at any time node on the local obstacle avoidance trajectory and the trajectory curvature at that time node satisfy the lateral acceleration constraint.
[0021] As a further technical solution, the process for handling the collision avoidance penalty item of the crane boom includes:
[0022] The chassis of the crane and the boom extending from the top of the crane are modeled as spatially physically isolated.
[0023] The real-time extension length and elevation angle data of the boom are obtained, and the three-dimensional attitude of the boom is projected onto the two-dimensional plane of the multi-layer cost map to generate a dynamic envelope box.
[0024] The time elastic band algorithm is used to predict the safe expansion zone of the dynamic envelope and static or dynamic obstacles in the future pose sequence inference. If the two overlap geometrically, an anti-collision cost is added to the preset multi-objective cost function to trigger the anti-collision penalty of the crane boom. The safe expansion zone is the area formed by expanding the physical contour of the obstacle outward by a preset safe buffer distance. The anti-collision cost is a penalty weight that tends to infinity set in the multi-objective cost function.
[0025] As a further technical solution, combining a dynamic obstacle layer in a multi-layer cost map includes the following dynamic obstacle trajectory prediction steps:
[0026] Target detection and attribute classification based on dynamic obstacles perceived from environmental point cloud and image data;
[0027] For vehicle targets with nonholonomic kinematic constraints, a constant turning rate and velocity model is used to predict the vehicle target's trajectory within a future time window.
[0028] For highly maneuverable and irregular human targets, a constant velocity model with Gaussian noise is used to predict the probability distribution of the human target's position as it expands radially over time within a future time window.
[0029] As a further technical solution, after predicting the movement trajectory or position probability distribution area of the dynamic obstacle within a future time window, an adaptive spatiotemporal safety boundary is also introduced, specifically:
[0030] When the shortest Euclidean distance between the dynamic envelope of the crane and the predicted position of the dynamic obstacle is less than the adaptive spatiotemporal safety boundary, the time difference between adjacent pose nodes is compressed to generate an active deceleration control command.
[0031] When deceleration alone cannot avoid a collision and the lateral acceleration constraint is met, lateral deformation of the local obstacle avoidance trajectory is triggered to generate a lane change avoidance command.
[0032] As a further technical solution, the expression for the adaptive spatiotemporal safety boundary is:
[0033]
[0034] in, For adaptive spatiotemporal safety boundaries, The basic physical radius of the dynamic obstacle. Let V be the velocity vector of the dynamic obstacle at the current moment. For speed sensitivity coefficient, This is the time uncertainty inflation coefficient. For the current moment, This refers to the predicted time point.
[0035] As a further technical solution, the initial global smooth path does not interfere with the crane operation safety boundary layer.
[0036] According to one aspect of the present invention, a hybrid-based The crane dynamic path planning and obstacle avoidance system integrated with TEB includes:
[0037] The multi-layer cost map construction module is used to construct a multi-layer cost map containing a static obstacle layer and a dynamic obstacle layer based on the environmental point cloud and image data of the construction site acquired in real time, and to expand the static obstacle layer and the dynamic obstacle layer according to the physical outline dimensions of the crane to generate a crane operation safety boundary layer.
[0038] The global prior planning module is used to establish a kinematic model based on the crane operation safety boundary layer and the nonholonomic kinematic constraints of the crane chassis, employing a hybrid approach. The algorithm performs a global node expansion search in the continuous state space of the kinematic model to generate an initial global smooth path;
[0039] The local dynamic optimization module is used to obtain an initial state set based on an initial global smooth path, and combine it with the dynamic obstacle layer in the multi-layer cost map. Under the constraint of a preset multi-objective cost function, the time elastic band algorithm is used to optimize the local trajectory deformation and time parameters of the initial state set to generate a local obstacle avoidance trajectory. The local obstacle avoidance trajectory is used to generate control commands to drive the crane.
[0040] Compared with the prior art, the beneficial effects of the present invention are:
[0041] 1. This invention achieves global mixing The algorithm strips away simple grid search and instead uses a dual heuristic function to deeply fuse the nonholonomic kinematic constraints of the crane. The generated path eliminates sharp corners from the source, eliminating the need for tedious post-processing smoothing. This significantly reduces wear and steering delay of the underlying mechanical mechanism and improves the dilemma of traditional algorithms being theoretically feasible but physically unexecutable.
[0042] 2. Unlike traditional TEB algorithms that treat robots as two-dimensional points or simple polygons, this invention innovatively limits the anti-sway lateral acceleration as a cost constraint and establishes an independent dynamic projection envelope for the boom. This prevents the crane from experiencing a fatal pendulum effect due to inertia when performing local emergency obstacle avoidance, while also eliminating the risk of high-altitude boom scraping in blind spots, thus achieving three-dimensional active safety protection against swaying under load and boom scraping.
[0043] 3. By introducing multimodal dynamic target prediction and adaptive spatiotemporal safety boundaries, this invention no longer avoids all dynamic obstacles in a one-size-fits-all manner. Combined with a human-like driving control strategy of "deceleration first, lane changing second", it significantly reduces "false deadlock" and ineffective emergency braking caused by workers moving around in the construction site environment while ensuring absolute safety. This improves the overall transfer efficiency and enhances the crane's ability to cope with high-level game theory in mixed human-machine environments. Attached Figure Description
[0044] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art 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.
[0045] Figure 1 The hybrid-based embodiment of the present invention is proposed Flowchart of crane dynamic path planning and obstacle avoidance method integrated with TEB.
[0046] Figure 2 This is a schematic diagram of the construction of a multi-layer cost map of the construction site and the projection of the dynamic envelope frame of the crane boom proposed in an embodiment of the present invention.
[0047] Figure 3 The hybrid proposed in the embodiments of the present invention A diagram comparing the global path generated by the algorithm with the traditional A path.
[0048] Figure 4 This is a schematic diagram of a 2D top-down view of a crane carrying loads and avoiding obstacles, as proposed in an embodiment of the present invention. Detailed Implementation
[0049] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0050] like Figure 1 As shown, this embodiment of the invention proposes a method based on hybrid The crane dynamic path planning and obstacle avoidance method integrated with TEB includes: Step S1, constructing a multi-layer cost map containing static and dynamic obstacle layers based on real-time acquired environmental point cloud and image data of the construction site, and expanding the static and dynamic obstacle layers according to the physical outline dimensions of the crane to generate a crane operation safety boundary layer; Step S2, in the global optimization stage, establishing a kinematic model based on the crane operation safety boundary layer in the multi-layer cost map and combining it with the nonholonomic kinematic constraints of the crane chassis, and adopting a hybrid approach. The algorithm performs a global node expansion search in the continuous state space of the kinematic model to generate an initial global smooth path that does not interfere with the crane's operational safety boundary layer. In step S3, during the local optimization stage, discrete trajectory points in the initial global smooth path are extracted as prior paths and transformed into an initial state set of a time elastic band. Combined with a dynamically updated obstacle layer, the time elastic band algorithm is used to optimize the local trajectory deformation and time parameters of the initial state set under the constraint of a preset multi-objective cost function, generating a local obstacle avoidance trajectory that satisfies the dynamic physical characteristics of the crane. The local obstacle avoidance trajectory is used to generate control commands to drive the crane.
[0051] Specifically, based on the local obstacle avoidance trajectory, the chassis system and superstructure of the crane execute motion control commands in a closed loop. During crane travel, steps S1 to S4 are executed in real-time with a set control cycle until dynamic path planning and obstacle avoidance are completed. The optimization of local trajectory deformation and time parameters includes load-bearing anti-sway, boom collision avoidance, and conventional obstacle avoidance and kinematic limit penalties. Real-time rolling is a real-time rolling optimization process. The crane does not wait for the local obstacle avoidance trajectory to complete before mapping; instead, it refreshes multiple layers of cost maps at a fixed frequency (e.g., 10Hz) while moving, synchronously outputting the local obstacle avoidance trajectory at high frequency through the TEB algorithm. This allows it to cope with the sudden appearance of workers or vehicles.
[0052] Specifically, in the global node expansion search during the global planning phase, a dual heuristic function is used to guide the search direction. This dual heuristic function compares the "curve distance satisfying the crane's kinematics while ignoring environmental obstacles" with the "shortest collision-free path distance based on a two-dimensional grid while ignoring the crane's kinematic constraints," and takes the maximum of the two as the heuristic cost for the current node in real time. This ensures global collision avoidance while guaranteeing that the path conforms to the crane's mechanical and physical limits from the source. The expression is:
[0053] (1)
[0054] in, It is a dual heuristic function. For the current search node, For the target node; To ignore environmental obstacles, the current node To the target node Reeds-Shepp curve distance; The subscript RS in the text indicates the Reeds-Shepp curve distance; To ignore the kinematic constraints of the crane, the current search node on the two-dimensional grid map To the target node The shortest collision-free path distance; The subscript 2D in the text indicates the path distance in a two-dimensional plane.
[0055] Specifically, the preset multi-objective cost function not only includes conventional trajectory following penalties, kinematic limit penalties, and obstacle avoidance penalties, but also innovatively introduces load-bearing anti-sway penalties and boom anti-collision penalties tailored to the operating characteristics of cranes. The expression for the preset multi-objective cost function is:
[0056] (2)
[0057] in, For the preset multi-objective cost function, For trajectory following penalty items, The weighting coefficients for the trajectory following penalty term. This is a kinematic limit penalty term. The weighting coefficients for the kinematic limit penalty term. This is a standard obstacle avoidance penalty item. The weighting coefficients for the standard obstacle avoidance penalty items. For cranes, a load-bearing anti-sway penalty item is provided. The weighting coefficient for the crane's load-bearing anti-sway penalty term. For crane boom collision avoidance penalties, This is the weighting coefficient for the collision avoidance penalty item of the crane boom.
[0058] Specifically, the process for handling the crane's anti-sway penalty term under load is as follows: By strictly constraining the linear velocity and trajectory curvature at each time point on the local obstacle avoidance trajectory, it is ensured that the lateral acceleration of the crane during load movement never exceeds the maximum allowable anti-sway safety threshold of the system; once the time elastic band algorithm predicts that local deformation will cause the lateral acceleration to exceed the limit, the cost of this penalty term increases exponentially, forcing the trajectory to smooth again. The linear velocity of the crane at any time point k on the trajectory... The trajectory curvature at that time point Satisfy lateral acceleration constraints:
[0059] (3)
[0060] in, This represents the maximum permissible lateral acceleration threshold when the crane is under load; when the solution deviates from this constraint, the cost of the crane under load anti-sway penalty term increases exponentially.
[0061] Specifically, the processing procedure for the crane boom anti-collision penalty is as follows: The crane chassis and its extended boom are spatially physically isolated and modeled; the system reads the boom's extension length and elevation angle data in real time, projects its three-dimensional attitude downwards onto the two-dimensional plane of the multi-layer cost map, and generates a dynamically changing envelope; if the TEB optimization solution predicts that the dynamic envelope overlaps with the safety expansion zone of a static or dynamic obstacle, the crane boom anti-collision penalty is triggered. Here, the safety expansion zone is the area formed by extending a preset safety buffer distance outwards from the obstacle's physical contour; the anti-collision penalty value is a penalty weight set to approach infinity in the multi-objective cost function.
[0062] Specifically, in the local dynamic obstacle avoidance process, dynamic obstacle trajectory prediction is introduced for dynamic obstacles:
[0063] Target detection and attribute classification are performed on perceived dynamic obstacles. For vehicle targets classified as having nonholonomic kinematic constraints, a constant turning rate and speed model is used to predict their deterministic movement trajectory within a future time window. For pedestrian targets classified as highly mobile and irregular, a constant speed model with Gaussian noise is used to predict their position probability distribution area that expands radially over time within a future time window.
[0064] Specifically, after predicting the deterministic movement trajectory or position probability distribution area of the dynamic obstacle within a future time window, the local planning algorithm constructs an adaptive spatiotemporal safety boundary that dynamically expands with the physical size of the dynamic obstacle, its real-time speed, and time uncertainty. When the shortest Euclidean distance between the crane's predicted envelope and the dynamic obstacle is less than the adaptive spatiotemporal safety boundary, the time elastic band algorithm follows a safety strategy of "deceleration first, lane change second" in generating control commands: it prioritizes compressing the time difference between adjacent pose nodes to perform active deceleration; when simple deceleration cannot avoid the obstacle and the lateral acceleration constraint is met, it triggers the lateral elastic deformation of the local trajectory to generate a lane change avoidance command.
[0065] The expression for the adaptive spatiotemporal safety boundary is:
[0066] (4)
[0067] in, For adaptive spatiotemporal safety boundaries, The basic physical radius of the dynamic obstacle. Let V be the velocity vector of the dynamic obstacle at the current moment. For speed sensitivity coefficient, This is the time uncertainty inflation coefficient. For the current moment, This refers to the predicted time point.
[0068] Specifically, in the field of construction machinery, the chassis system of a crane is mainly responsible for the vehicle's movement and steering, while the lifting mechanism (i.e., the superstructure system, including the boom, slewing table, luffing cylinders, etc.) is responsible for the lifting action. The two are two different mechanisms at the underlying electrical control level. When the crane avoids obstacles, not only the chassis needs to move, but the lifting mechanism of the superstructure system also needs to cooperate (for example, the coordinated control during anti-swaying).
[0069] Specifically, in this embodiment of the invention, the device is deployed on an in-vehicle industrial control computer, and the implementation steps are as follows:
[0070] I. System Initialization and Multi-Source Data Fusion Sensing
[0071] After the crane is powered on and starts up and receives the relocation and hoisting task, the system enters the initialization and sensing phase:
[0072] Sensor time synchronization and spatial calibration: The vehicle-mounted industrial control computer achieves microsecond-level time synchronization of 3D LiDAR, dual-antenna GNSS-RTK and IMU through a hard triggering mechanism; and calls joint calibration parameters to unify all sensor data under the coordinate system of the crane chassis center.
[0073] Dynamic construction and updating of multi-layer cost maps. Static and dynamic layers: After filtering out the ground using LiDAR point clouds, scaffolding and other structures are projected as two-dimensional static grids; simultaneously, target detection data fused from visual cameras and radar is received, and the grid occupancy of moving objects is updated in real time. For example... Figure 2 As shown, the boom projection anti-collision layer is a cross-sectional schematic diagram of the multi-layer cost map construction and the dynamic envelope projection of the boom at the construction site. The system not only performs chassis expansion but also reads real-time data from the winch and luffing cylinder encoders to calculate the current boom length and elevation angle in three-dimensional space. For example... Figure 2 As shown in the projection relationship, the geometric model of the 3D boom is projected vertically along the Z-axis onto the 2D cost map, generating a dynamic envelope polygon that changes in real time with the movement of the chassis and the amplitude of the boom. If the envelope polygon coincides with a high-altitude obstacle in the prediction, the highest level of collision avoidance warning is triggered.
[0074] II. Hybrid systems incorporating chassis non-holonomic constraints Global path planning
[0075] After the driver or the central control room issues the target work point, the industrial control computer triggers the global planner. Figure 3 The article provides a direct comparison between the traditional A algorithm and the hybrid algorithm in the embodiments of this invention. Differences in the global paths generated by the algorithm.
[0076] Kinematic state space construction: Establishing the Ackermann steering kinematic model of the crane chassis:
[0077] , , (5)
[0078] in, , These represent the linear velocities of the crane in the x and y directions, respectively. This indicates the heading angle of the crane chassis (i.e., the angle between the direction of travel and the coordinate axis). This indicates the angular velocity of the crane chassis. Wheelbase Set the absolute minimum turning radius for the front wheel steering angle. .
[0079] Dual heuristic search: hybrid The algorithm uses a dual heuristic function when expanding nodes, combining... Figure 3 It can be seen that tradition The path generated by the algorithm contains a large number of right angles or acute angles, which the crane cannot execute directly; however, the initial global smooth path generated by the embodiment of the present invention, due to the introduction of Reeds-Shepp curve distance heuristic, satisfies the minimum turning radius constraint of the crane from the source, ensuring that the chassis can smoothly drive into the target point.
[0080] III. Introducing TEB Local Elastic Optimization for Anti-Sway and Boom Collision Prevention
[0081] Extract the local look-ahead window of the global path as the initial state of the time elastic band. Construct a multi-objective cost function. Perform iterative optimization:
[0082] (6)
[0083] Among them, the load-bearing anti-sway penalty item Extract the linear velocity of each node on the elastic band. With path curvature Forced application of lateral acceleration constraints:
[0084] (7)
[0085] When the detected lateral acceleration approaches a threshold related to the length of the suspension rope At this time, the penalty increases sharply, forcing the trajectory to reduce speed or increase the turning radius to avoid the heavy object swinging violently.
[0086] IV. Dynamic Obstacle Multimodal Prediction and Layered Avoidance Logic
[0087] For highly dynamic scenarios involving both humans and machines, this invention incorporates a deeply designed "perception and tracking-prediction-layered game" logic. For example... Figure 4 The diagram shown illustrates the local path deformation optimization when the system encounters dynamic obstacles. The steps are as follows:
[0088] S1, Multi-target tracking and attribute classification
[0089] Extended Kalman filtering is used to continuously track dynamic targets, classifying them into targets subject to nonholonomic constraints and highly maneuverable, unpredictable targets.
[0090] S2, Differentiated Trajectory Deduction
[0091] For vehicles, a CTRV model is used to predict trajectories; for pedestrians, a noisy constant velocity (CV) model is used. For example... Figure 4 As shown in the trumpet-shaped area in front of the pedestrian, the model outputs an ellipse of position probability distribution that expands over time.
[0092] S3, Calculate the adaptive spatiotemporal safety boundary
[0093] Adaptive spatiotemporal safety boundary for dynamic calculation of obstacles :
[0094] (8)
[0095] in, This is the inertia compensation coefficient, used to adjust the weight of the mass effect; This refers to the current actual load capacity of the crane. This is a preset mapping function between the load mass and the increment of the braking or avoidance safety distance; This is the inertia compensation term for the load mass of the crane. The calculation result is... Figure 4 An elliptical safety bubble surrounding a dynamic obstacle.
[0096] S4, Crane Layered Dynamic Alternation State Machine Decision
[0097] The system calculates the shortest distance between the crane's predicted envelope and the predicted position of the dynamic target. Execute the three-level hierarchical logic:
[0098] Level 1 (Safe Cruise): When At that time, follow the optimal time trajectory, where, This is the preset safety buffer distance.
[0099] Level 2 (Active Deceleration and Spatial Yield): When there is a potential intersection risk, the time penalty adjustment is triggered first to compress the time difference between adjacent pose nodes and output a smooth deceleration command to allow the obstacle to pass first.
[0100] Level 3 (Elastic Deformation Avoidance / Flexible Emergency Stop): When an obstacle suddenly enters the crane's safety envelope:
[0101] If the adjacent spaces meet the conditions, the time elastic band algorithm will strictly satisfy the anti-sway constraint. Under the premise of [condition], lateral elastic deformation is triggered. For example... Figure 4 As shown in the solid line obstacle avoidance trajectory that deviates from the original dashed global path, the crane smoothly bypasses the bubble and returns to the global path.
[0102] If the space is too narrow to change lanes, the system immediately cuts off the time elastic band output, triggers the anti-sway flexible emergency stop braking curve, and controls the chassis to stop smoothly.
[0103] V. Smooth Control Command Processing and Human-Machine Interaction Closed Loop
[0104] The optimized local obstacle avoidance trajectory is parsed into linear and angular velocity commands for the chassis drive motors and transmitted via the CAN bus. Simultaneously, the in-vehicle display renders the data in real time. Figure 2 and Figure 4 The system displays a 3D crane model, a local point cloud map, a time-elastic band prediction blue trajectory line, and a red highlighted dynamic obstacle prediction bubble. When the system performs a level 3 emergency avoidance maneuver, the interface simultaneously issues an audible and visual alarm; the driver can take over at any time via the joystick, and the system automatically cuts off the algorithm output, achieving low-level safety redundancy control for human-machine co-driving.
[0105] Based on the above embodiments, embodiments of the present invention provide a hybrid-based A crane dynamic path planning and obstacle avoidance system integrated with TEB, which is used to perform the hybrid-based methods described in the above embodiments. A crane dynamic path planning and obstacle avoidance method integrated with TEB.
[0106] The system includes: a multi-layer cost map construction module, used to construct a multi-layer cost map containing static and dynamic obstacle layers based on real-time acquired environmental point cloud and image data of the construction site, and to expand the static and dynamic obstacle layers according to the physical outline dimensions of the crane to generate a crane operation safety boundary layer; and a global prior planning module, used to establish a kinematic model based on the crane operation safety boundary layer and the nonholonomic kinematic constraints of the crane chassis, using a hybrid approach. The algorithm performs a global node expansion search in the continuous state space of the kinematic model to generate an initial global smooth path. The local dynamic optimization module is used to obtain an initial state set based on the initial global smooth path, and combined with the dynamic obstacle layer in the multi-layer cost map, it uses the time elastic band algorithm under the constraint of a preset multi-objective cost function to optimize the local trajectory deformation and time parameters of the initial state set to generate a local obstacle avoidance trajectory. The local obstacle avoidance trajectory is used to generate control commands to drive the crane.
[0107] The embodiments of the present invention provide a hybrid-based The existing crane dynamic path planning and obstacle avoidance system integrated with TEB struggles to simultaneously ensure global path optimization while also considering the nonholonomic constraints of large cranes and proactive obstacle avoidance in complex dynamic environments. Furthermore, it fails to effectively address issues of anti-swaying under load and collision avoidance in the boom space. This system employs several modules and utilizes global hybridization... The algorithm strips away simple grid search and instead uses a dual heuristic function to deeply fuse the nonholonomic kinematic constraints of the crane. The generated path eliminates sharp angles from the source, limits the anti-sway lateral acceleration to a cost constraint, and establishes an independent dynamic projection envelope for the boom. It also introduces multimodal dynamic target prediction and adaptive spatiotemporal safety boundaries, so as to no longer avoid all dynamic obstacles in a one-size-fits-all manner.
[0108] Finally, it should be noted that the above specific embodiments are merely representative examples of the present invention. Obviously, the present invention is not limited to the above specific embodiments and many variations are possible. Any simple modifications, equivalent changes, and alterations made to the above specific embodiments based on the technical essence of the present invention should be considered within the protection scope of the present invention.
Claims
1. A hybrid-based The crane dynamic path planning and obstacle avoidance method integrated with TEB is characterized by, include: Step S1: Based on the real-time acquired environmental point cloud and image data of the construction site, construct a multi-layer cost map containing a static obstacle layer and a dynamic obstacle layer, and expand the static obstacle layer and the dynamic obstacle layer according to the physical outline size of the crane to generate a crane operation safety boundary layer. Step S2: Based on the crane operation safety boundary layer, and combined with the nonholonomic kinematic constraints of the crane chassis, establish a kinematic model, and adopt a hybrid approach. The algorithm performs a global node expansion search in the continuous state space of the kinematic model to generate an initial global smooth path; Step S3: Based on the initial global smooth path, an initial state set is obtained. Combined with the dynamic obstacle layer in the multi-layer cost map, the initial state set is subjected to local trajectory deformation and time parameter optimization under the constraint of a preset multi-objective cost function using the time elastic band algorithm to generate a local obstacle avoidance trajectory. The local obstacle avoidance trajectory is used to generate control commands to drive the crane.
2. A hybrid-based method according to claim 1 The crane dynamic path planning and obstacle avoidance method integrated with TEB is characterized by, The mixture The algorithm uses a dual heuristic function, expressed as follows: , in, It is a dual heuristic function. For the current search node, For the target node; To ignore environmental obstacles, the current node To the target node Reeds-Shepp curve distance; The subscript RS in the text indicates the Reeds-Shepp curve distance; To ignore the kinematic constraints of the crane, the current search node on the two-dimensional grid map To the target node The shortest collision-free path distance; The subscript 2D in the text indicates the path distance in a two-dimensional plane.
3. A hybrid-based method according to claim 1 The crane dynamic path planning and obstacle avoidance method integrated with TEB is characterized by, The expression for the preset multi-objective cost function is: , in, For the preset multi-objective cost function, For trajectory following penalty items, The weighting coefficients for the trajectory following penalty term. This is a kinematic limit penalty term. The weighting coefficients for the kinematic limit penalty term. This is a standard obstacle avoidance penalty item. The weighting coefficients for the standard obstacle avoidance penalty items. For cranes, a load-bearing anti-sway penalty item is provided. The weighting coefficient for the crane's load-bearing anti-sway penalty term. For crane boom collision avoidance penalties, This is the weighting coefficient for the collision avoidance penalty item of the crane boom.
4. A hybrid-based method according to claim 3 The crane dynamic path planning and obstacle avoidance method integrated with TEB is characterized by, The constraint conditions for the crane's load-bearing anti-sway penalty term are as follows: Constraints are imposed on the linear velocity and trajectory curvature of the local obstacle avoidance trajectory so that the linear velocity of the crane at any time node on the local obstacle avoidance trajectory and the trajectory curvature at that time node satisfy the lateral acceleration constraint.
5. A hybrid-based method according to claim 3 The crane dynamic path planning and obstacle avoidance method integrated with TEB is characterized by, The process for handling the crane boom collision avoidance penalty includes: The chassis of the crane and the boom extending from the top of the crane are modeled as spatially physically isolated. The real-time extension length and elevation angle data of the boom are obtained, and the three-dimensional attitude of the boom is projected onto the two-dimensional plane of the multi-layer cost map to generate a dynamic envelope box. The time elastic band algorithm is used to predict the safe expansion zone of the dynamic envelope and static or dynamic obstacles in the future pose sequence inference. If the two overlap geometrically, an anti-collision cost is added to the preset multi-objective cost function to trigger the anti-collision penalty of the crane boom. The safe expansion zone is the area formed by expanding the physical contour of the obstacle outward by a preset safe buffer distance. The anti-collision cost is a penalty weight that tends to infinity set in the multi-objective cost function.
6. A hybrid-based method according to claim 1 The crane dynamic path planning and obstacle avoidance method integrated with TEB is characterized by, Combining the dynamic obstacle layers in the multi-layered cost map, the following dynamic obstacle trajectory prediction steps are included: Target detection and attribute classification based on dynamic obstacles perceived from environmental point cloud and image data; For vehicle targets with nonholonomic kinematic constraints, a constant turning rate and velocity model is used to predict the vehicle target's trajectory within a future time window. For highly maneuverable and irregular human targets, a constant velocity model with Gaussian noise is used to predict the probability distribution of the human target's position as it expands radially over time within a future time window.
7. A hybrid-based method according to claim 6 The crane dynamic path planning and obstacle avoidance method integrated with TEB is characterized by, After predicting the probability distribution region of the movement trajectory or position of dynamic obstacles within a future time window, the method also includes introducing an adaptive spatiotemporal safety boundary, specifically: When the shortest Euclidean distance between the dynamic envelope of the crane and the predicted position of the dynamic obstacle is less than the adaptive spatiotemporal safety boundary, the time difference between adjacent pose nodes is compressed to generate an active deceleration control command. When deceleration alone cannot avoid a collision and the lateral acceleration constraint is met, lateral deformation of the local obstacle avoidance trajectory is triggered to generate a lane change avoidance command.
8. A hybrid-based method according to claim 7 The crane dynamic path planning and obstacle avoidance method integrated with TEB is characterized by, The expression for the adaptive spatiotemporal safety boundary is: , in, For adaptive spatiotemporal safety boundaries, The basic physical radius of the dynamic obstacle. Let V be the velocity vector of the dynamic obstacle at the current moment. For speed sensitivity coefficient, This is the time uncertainty inflation coefficient. For the current moment, This refers to the predicted time point.
9. A hybrid-based method according to claim 1 The crane dynamic path planning and obstacle avoidance method integrated with TEB is characterized by, The initial global smooth path does not interfere with the crane operation safety boundary layer.
10. A hybrid-based The crane dynamic path planning and obstacle avoidance system integrated with TEB is characterized by, include: The multi-layer cost map construction module is used to construct a multi-layer cost map containing a static obstacle layer and a dynamic obstacle layer based on the environmental point cloud and image data of the construction site acquired in real time, and to expand the static obstacle layer and the dynamic obstacle layer according to the physical outline dimensions of the crane to generate a crane operation safety boundary layer. The global prior planning module is used to establish a kinematic model based on the crane operation safety boundary layer and the nonholonomic kinematic constraints of the crane chassis, employing a hybrid approach. The algorithm performs a global node expansion search in the continuous state space of the kinematic model to generate an initial global smooth path; The local dynamic optimization module is used to obtain an initial state set based on an initial global smooth path, and combine it with the dynamic obstacle layer in the multi-layer cost map. Under the constraint of a preset multi-objective cost function, the time elastic band algorithm is used to optimize the local trajectory deformation and time parameters of the initial state set to generate a local obstacle avoidance trajectory. The local obstacle avoidance trajectory is used to generate control commands to drive the crane.