Intelligent obstacle avoidance and operation range planning method and system for manual telescopic arm of truck-mounted crane

Abstract

The invention provides an intelligent obstacle avoidance and operation range planning method and system for a manual telescopic arm of a truck-mounted crane, and relates to the technical field of truck-mounted crane obstacle avoidance, and the method comprises the following steps: collecting environment point cloud data, truck-mounted crane attitude data and obstacle distance data, constructing a dynamic environment map, and identifying obstacle features; dividing operation areas based on the dynamic environment map and the obstacle state data, and establishing an operation space safety level model; in combination with the working space safety level model and the obstacle state data, an obstacle avoidance candidate path meeting load constraints is generated, an optimal path is selected and converted into an obstacle avoidance track and a control instruction, and finally cantilever crane obstacle avoidance is achieved; by monitoring the distance between the boom and the obstacle in real time, hierarchical control is executed, and the safety and efficiency of operation of the manual telescopic boom of the truck-mounted crane are effectively improved.

Description

Technical Field

[0001] The present invention relates to the technical field of obstacle avoidance for truck mounted cranes, and in particular to a method and system for intelligent obstacle avoidance and operation range planning for a manual telescopic arm of a truck mounted crane. Background Art

[0002] Truck-mounted cranes are mobile cranes installed on a car chassis and are widely used in construction, logistics and other fields. With the growing demand for urban construction and industrial development, the operating environment of truck-mounted cranes is becoming increasingly complex, especially when operating in narrow urban spaces, high-voltage line dense areas and environments with multiple obstacles, which puts higher requirements on operating safety and efficiency. At present, truck-mounted cranes mainly rely on the operator's visual judgment and experience operation, and lack intelligent environmental perception and obstacle avoidance planning capabilities.

[0003] However, there are several common problems in the existing technology. Traditional truck-mounted cranes are unable to perceive and model complex dynamic environments in real time, and it is difficult to accurately identify and track the status of obstacles; the safety level division of the working space is too rough and fails to fully consider the load capacity distribution and the impact of obstacles; the obstacle avoidance path planning method is relatively simple, and it is difficult to take into account path smoothness and load constraints while ensuring safety; there is a lack of real-time monitoring and hierarchical control mechanism based on multi-sensor information fusion, which poses a major safety hazard.

[0004] In summary, it is necessary to solve the technical problems of inaccurate environmental perception, unreasonable division of working space, insufficient obstacle avoidance planning ability and lack of safety monitoring mechanism when the truck crane is operating in a complex environment. The present invention realizes the safe and intelligent operation of the truck crane in a complex environment, improves the operating efficiency and safety, and can solve the problems in the prior art by establishing an environmental perception system based on multi-source information fusion, a hierarchical working space planning method, an intelligent obstacle avoidance path planning algorithm and a real-time safety monitoring mechanism. Summary of the invention

[0005] The embodiment of the present invention provides a method and system for intelligent obstacle avoidance and operation range planning of a manual telescopic arm of a truck-mounted crane, which can solve the problems in the prior art.

[0006] According to a first aspect of the embodiments of the present invention,

[0007] Provided is a method for intelligent obstacle avoidance and operation range planning of a manual telescopic arm of a truck-mounted crane, comprising:

[0008] Collect environmental point cloud data, crane posture data and obstacle distance data around the truck crane; use bilateral filtering algorithm to reduce noise and perform point cloud registration on the point cloud data, and then obtain a scene map through a dynamic positioning mapping algorithm; use quaternion posture solution method to process the crane posture data, perform posture correction on the scene map, and obtain an environmental correction model; use iterative closest point registration to spatially fuse the environmental correction model with the obstacle distance data to construct a dynamic environmental map; use target detection algorithm combined with Kalman filtering to identify obstacle features in the dynamic environmental map and obtain obstacle status data;

[0009] Based on the dynamic environment map and in combination with the obstacle state data, the initial operation area is divided by a region boundary generation algorithm, and a convex hull decomposition algorithm is used to determine the final operation area; a boom kinematic model is established based on a joint geometric parameter method, and the safe load capacity distribution within the final operation area is calculated. According to the safe load capacity distribution, the final operation area is graded to establish an operation space safety level model;

[0010] Based on the working space safety level model and the obstacle status data, a fast random tree connection method combined with dynamic programming is used to generate candidate obstacle avoidance paths that meet load constraints; the optimal obstacle avoidance path is selected according to the comprehensive score calculated based on the path safety and load margin, and the optimal obstacle avoidance path is converted into an obstacle avoidance trajectory and control instructions through a quintic polynomial interpolation algorithm; a real-time distance field algorithm is used to monitor the distance between the boom and the obstacle, and prompts, warnings and control instructions are executed based on a preset hierarchical control threshold.

[0011] In an optional embodiment,

[0012] After the point cloud data is subjected to noise reduction and point cloud registration processing by using a bilateral filtering algorithm, a scene map is obtained by using a dynamic positioning mapping algorithm; the truck crane attitude data is processed by using a quaternion attitude solution method, and the scene map is subjected to posture correction to obtain an environmental correction model including:

[0013] Each sampling point in the point cloud data is sequentially used as a point to be processed, and the neighboring points of the point to be processed are searched within a preset radius, and the spatial distance weight and the intensity value weight of the point to be processed and the corresponding neighboring points are respectively calculated, and the product of the spatial distance weight and the intensity value weight is used as the comprehensive weight, and the position of the point to be processed is updated based on the comprehensive weight to obtain the filtered point cloud data;

[0014] Calculate the normal vector and principal curvature value of each point in the filtered point cloud data, select the local extreme point of the principal curvature value as the feature point, construct the local descriptor of the feature point, perform feature matching on the local descriptor through fast neighbor search, remove the wrong matching points in combination with the random sampling consistency algorithm, determine the retained matching points, calculate the pose transformation matrix based on the retained matching points, and obtain the point cloud registration data;

[0015] Determine the initial pose based on the pose transformation matrix, extract the local plane features in the point cloud registration data to construct a pose graph, calculate the point cloud distance field value between consecutive frames, compare the point cloud distance field value with the adaptive threshold to identify the dynamic point, assign attenuation weights to the local plane features corresponding to the dynamic point, construct a weighted pose optimization equation, solve it to obtain the optimized pose, transform the point cloud of the current frame to the global coordinate system through the optimized pose, and update the scene map;

[0016] Based on the optimized posture and the point cloud motion information in the scene map, a quaternion equation describing the spatial posture is constructed, the quaternion equation is numerically integrated to obtain a predicted posture, and an error state equation is constructed based on the deviation between the predicted posture and the actual observed posture and solved to obtain a posture quaternion;

[0017] Calculate each element of the rotation matrix according to the posture quaternion, perform coordinate transformation on the point cloud position and normal vector in the scene map through the rotation matrix, update local plane parameters based on the transformed point cloud position and normal vector, reconstruct the spatial position relationship of the feature points, and generate an environment correction model.

[0018] In an optional embodiment,

[0019] The environment correction model and the obstacle distance data are spatially fused through iterative closest point registration to construct a dynamic environment map; obstacle features in the dynamic environment map are identified through a target detection algorithm combined with Kalman filtering, and obstacle status data is obtained, including:

[0020] Taking the feature point set in the environmental correction model as the first feature point set, taking the feature point set in the obstacle distance data as the second feature point set, taking each feature point in the first feature point set as a reference feature point, searching for the nearest neighbor point in the second feature point set, and constructing a point pair set with the reference feature point and the nearest neighbor point;

[0021] Calculate the centroid coordinates based on the point pair set, calculate the relative position of each point in the point pair set relative to the centroid coordinates, construct a covariance matrix of the relative positions, and perform singular value decomposition on the covariance matrix to obtain a rotation matrix and a translation vector;

[0022] Based on the rotation matrix and the translation vector, coordinate transformation is performed on the reference feature point, and the nearest neighbor point search, centroid coordinate calculation and coordinate transformation are repeatedly performed until the position change of the reference feature point is less than a preset threshold value, thereby obtaining a dynamic environment map;

[0023] Clustering and segmenting the point cloud in the dynamic environment map, extracting the position center, main direction vector and size parameter of each cluster, constructing a feature vector, and establishing an obstacle state vector including a position vector, a velocity vector and an acceleration vector based on the feature vector;

[0024] A state transfer equation is constructed according to the obstacle state vector at adjacent moments, and an observation equation is constructed by taking the characteristic vector as an observation value. Based on the state transfer equation and the observation equation, a deviation between a predicted value and an observed value of the obstacle state vector is calculated, and the obstacle state vector is updated using the deviation to obtain obstacle state data.

[0025] In an optional embodiment,

[0026] Based on the dynamic environment map and in combination with the obstacle status data, dividing the initial operation area by using an area boundary generation algorithm includes:

[0027] Calculate the outer polygon of the point cloud data in the dynamic environment map, select three non-collinear feature points within the scope of the outer polygon to construct a triangular mesh, insert the remaining point cloud data into the triangular mesh in sequence, iteratively adjust the triangular mesh by maximizing the minimum inner angle of the triangle, and if the minimum inner angle of the triangle after adjustment is greater than the minimum inner angle of the triangle before adjustment, update the connection relationship of the triangular mesh until all the point cloud data are added to the triangular mesh;

[0028] In the triangular mesh, for each triangle, mark it as the current triangle, determine whether there is a circle with a preset radius that passes through the three vertices of the current triangle, and the interior of the circle does not contain any point in the point cloud data. If so, the current triangle is a valid triangle; extract the edge segments of all the valid triangles, determine the edge segment set, take the segment that appears once in the edge segment set as the target boundary segment, and reorganize the target boundary segment into a closed area boundary curve in a way that the endpoints are connected;

[0029] Based on the position vector and velocity vector in the obstacle state data, the movement trajectory of the obstacle is predicted within a preset time window, and the expansion area containing the current position and movement trajectory of the obstacle is calculated as the obstacle influence area. The area enclosed by the area boundary curve is superimposed with the obstacle influence area to obtain the initial operation area.

[0030] In an optional embodiment,

[0031] Using the convex hull decomposition algorithm, the final operation area is determined to include:

[0032] Connect any two convex vertices in the initial operation area, check whether the line between the two convex vertices has an intersection with the area boundary curve, and construct the lines without intersections into a connected graph;

[0033] Selecting a concave vertex with the smallest inner angle in the initial operation area as a segmentation starting point, selecting an optimal segmentation line in the connected graph to divide the area into a first sub-area and a second sub-area, performing segmentation steps on the first sub-area and the second sub-area respectively until each sub-area after segmentation becomes a convex polygon, obtaining a convex polygon sub-area, and forming all the convex polygon sub-areas into an area set;

[0034] Calculate the area change of adjacent convex polygonal sub-regions with common edges in the region set after merging. When the merged region remains a convex polygon and the area change is less than a preset change threshold, merge the adjacent convex polygonal sub-regions into a new sub-region. Repeat the merging until there are no sub-regions that can be merged, and obtain the final working area.

[0035] In an optional embodiment,

[0036] Based on the workspace safety level model and the obstacle state data, a fast random tree connection method combined with dynamic programming is used to generate an obstacle avoidance candidate path that meets the load constraint, including:

[0037] Establishing a state space of the truck crane, the state space including configuration parameters and motion parameters of the truck crane, and establishing a state transfer cost function based on the state space, the state transfer cost function including a distance cost between state points, a path curvature cost, and a safety level cost based on a work space safety level model;

[0038] In the state space, determine a starting state point and a target state point, respectively establish bidirectional random trees with the starting state point and the target state point as root nodes, including a forward random tree and a reverse random tree, and randomly sample in the state space to obtain candidate state points;

[0039] Taking the forward random tree or the reverse random tree as the current random tree, determining the tree node closest to the candidate state point in the current random tree, and extending a preset distance from the tree node toward the candidate state point to obtain an extended node;

[0040] Based on the workspace safety level model, determine the safety level of the extension node, and based on the obstacle status data, determine whether the path from the tree node to the extension node intersects with an obstacle, and when the safety level of the extension node meets the preset area and does not intersect with the obstacle, add the extension node to the current random tree;

[0041] Determine whether there is a connection path between the bidirectional random trees, generate an initial obstacle avoidance path when there is a connection path, sample at the path nodes of the initial obstacle avoidance path to obtain a set of candidate nodes, calculate the path cost of each node in the set of candidate nodes based on the state transition cost function; and generate a set of obstacle avoidance candidate paths that meet the load constraint based on the path cost using a dynamic programming method.

[0042] In an optional embodiment,

[0043] The optimal obstacle avoidance path is selected according to the comprehensive score calculated based on the path safety and load margin, and the optimal obstacle avoidance path is converted into an obstacle avoidance trajectory and control instructions through a quintic polynomial interpolation algorithm, including:

[0044] For each candidate path in the candidate path set, a discrete point sequence on the candidate path is calculated, and for each discrete point in the discrete point sequence, a distance from the discrete point to the nearest obstacle is calculated, and a ratio of the distance to a preset safety distance threshold is used as the path safety;

[0045] For each discrete point in the discrete point sequence, a ratio of the rated lifting weight to the actual lifting weight of the discrete point is calculated as a safety factor, and a ratio of the safety factor to a preset reference safety factor is used as a load margin;

[0046] Performing a weighted combination of the path safety and the load margin to obtain a path comprehensive score, and selecting a candidate path with the highest path comprehensive score as the optimal obstacle avoidance path;

[0047] Selecting two adjacent path nodes on the optimal obstacle avoidance path, and establishing a quintic polynomial trajectory equation based on position constraints, velocity constraints, and acceleration constraints of the two adjacent path nodes;

[0048] The quintic polynomial trajectory equation is sampled to obtain the position sequence, velocity sequence and acceleration sequence of the end effector in Cartesian space; the position sequence is transformed through inverse kinematics to obtain the angle sequence in the joint space, and the angle sequence is numerically differentiated to obtain the velocity sequence in the joint space; the angle sequence and the velocity sequence are converted into a motor speed sequence and a cylinder extension and retraction speed sequence to generate corresponding control instructions.

[0049] According to a second aspect of the embodiments of the present invention,

[0050] Provided is an intelligent obstacle avoidance and operation range planning system for a manual telescopic arm of a truck-mounted crane, including:

[0051] The first unit is used to collect environmental point cloud data, attitude data of the truck crane and obstacle distance data around the truck crane; after the point cloud data is subjected to noise reduction and point cloud registration processing by using a bilateral filtering algorithm, a scene map is obtained by using a dynamic positioning mapping algorithm; the attitude data of the truck crane is processed by using a quaternion attitude solution method, and the scene map is subjected to posture correction to obtain an environmental correction model; the environmental correction model is spatially fused with the obstacle distance data by iterative closest point registration to construct a dynamic environmental map; obstacle features in the dynamic environmental map are identified by combining a target detection algorithm with a Kalman filter to obtain obstacle status data;

[0052] The second unit is used to divide the initial working area by a regional boundary generation algorithm based on the dynamic environment map and the obstacle state data, and determine the final working area by a convex hull decomposition algorithm; establish a boom kinematic model based on a joint geometric parameter method, calculate the safe load capacity distribution within the final working area, classify the final working area according to the safe load capacity distribution, and establish a working space safety level model;

[0053] The third unit is used to generate a candidate obstacle avoidance path that meets the load constraint based on the working space safety level model and the obstacle status data by using a fast random tree connection method combined with dynamic programming; select the optimal obstacle avoidance path according to the comprehensive score calculated based on the path safety and load margin, and convert the optimal obstacle avoidance path into an obstacle avoidance trajectory and control instructions through a quintic polynomial interpolation algorithm; use a real-time distance field algorithm to monitor the distance between the boom and the obstacle, and execute prompts, warnings and control instructions based on a preset hierarchical control threshold.

[0054] According to a third aspect of the embodiments of the present invention,

[0055] An electronic device is provided, comprising:

[0056] processor;

[0057] a memory for storing processor-executable instructions;

[0058] The processor is configured to call the instructions stored in the memory to execute the aforementioned method.

[0059] A fourth aspect of the embodiments of the present invention is:

[0060] A computer-readable storage medium is provided, on which computer program instructions are stored. When the computer program instructions are executed by a processor, the aforementioned method is implemented.

[0061] In an embodiment of the present invention, by real-time monitoring of the distance between the boom and the obstacle, and executing prompts, warnings and control instructions based on the graded control threshold, collision accidents can be effectively avoided and the safety of the operation can be guaranteed; the obstacle avoidance path and the operating range can be intelligently planned to reduce manual intervention, realize automated operation and improve operating efficiency; the operating area is graded according to the safe load capacity distribution, and the obstacle avoidance path is planned in combination with the load constraints to give full play to the load capacity of the truck crane and optimize the operation plan. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] Figure 1 It is a flow chart of a method for intelligent obstacle avoidance and operation range planning of a manual telescopic arm of a truck-mounted crane according to an embodiment of the present invention;

[0063] Figure 2 It is a structural schematic diagram of the intelligent obstacle avoidance and operation range planning system of the manual telescopic arm of a truck-mounted crane according to an embodiment of the present invention. DETAILED DESCRIPTION

[0064] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0065] The technical solution of the present invention is described in detail with specific embodiments below. The following specific embodiments can be combined with each other, and the same or similar concepts or processes may not be described in detail in some embodiments.

[0066] Figure 1 FIG. 1 is a flow chart of a method for intelligent obstacle avoidance and operation range planning of a manual telescopic arm of a truck-mounted crane according to an embodiment of the present invention. Figure 1 As shown, the method includes:

[0067] S101. Collect environmental point cloud data, attitude data of the truck crane and obstacle distance data around the truck crane; use bilateral filtering algorithm to perform noise reduction and point cloud registration on the point cloud data, and then obtain a scene map through a dynamic positioning mapping algorithm; use quaternion attitude solution method to process the attitude data of the truck crane, perform posture correction on the scene map, and obtain an environmental correction model; spatially fuse the environmental correction model with the obstacle distance data through iterative closest point registration to construct a dynamic environmental map; identify obstacle features in the dynamic environmental map through a target detection algorithm combined with Kalman filtering to obtain obstacle status data;

[0068] First, at the data collection level, the system simultaneously acquires three types of key data: 3D point cloud data of the surrounding environment is collected through sensors such as laser radar; spatial posture information of the truck crane is collected through attitude sensors; and distance information of obstacles is collected through distance measurement sensors. These raw data provide the basis for subsequent processing.

[0069] Secondly, at the point cloud processing level, the collected raw point cloud data is subjected to bilateral filtering. This process not only retains the edge features of the point cloud, but also effectively removes noise points. The filtered point cloud is then registered to align the point cloud data collected at different times to a unified coordinate system, and a preliminary scene map is generated through a dynamic positioning and mapping algorithm.

[0070] The third is the posture correction level, which uses the quaternion method to solve the posture data of the truck crane to obtain accurate posture information. This posture information is used to correct the previously generated scene map to ensure the accuracy of the map in spatial position and form an environmental correction model.

[0071] The fourth is the data fusion level, which fuses the corrected environment model with the obstacle distance data. This fusion process uses an iterative closest point registration method to unify the data from two different sources into the same coordinate system to build a dynamically updated environment map.

[0072] Finally, at the obstacle recognition level, based on the dynamic environment map, obstacles are identified using the target detection algorithm, and the Kalman filter is used to estimate and predict the motion state of the obstacle, ultimately outputting the obstacle's position, speed and other status information.

[0073] This entire process realizes the transformation from raw data to effective information. Through the collection, processing and fusion of multi-source data, an accurate environmental perception model is finally established to provide reliable data support for subsequent obstacle avoidance planning.

[0074] In this embodiment, the accuracy and reliability of environmental perception are improved, and an accurate scene map and environmental correction model are constructed through point cloud data processing and dynamic positioning and mapping algorithms. Efficient fusion of dynamic environmental information is achieved, and the corrected environmental model is combined with obstacle distance data to construct a real-time updated dynamic environmental map. The accuracy of obstacle recognition is enhanced, and the state information of obstacles is accurately obtained by using target detection algorithms and Kalman filtering, providing reliable support for subsequent safety control.

[0075] S102. Based on the dynamic environment map and in combination with the obstacle status data, the initial operation area is divided by a region boundary generation algorithm, and a convex hull decomposition algorithm is used to determine the final operation area; a boom kinematic model is established based on a joint geometric parameter method, and the safe load capacity distribution within the final operation area is calculated. According to the safe load capacity distribution, the final operation area is graded to establish an operation space safety level model;

[0076] After obtaining the dynamic environment map and obstacle status data, the system first needs to determine the effective operating range of the truck crane. This process uses the area boundary generation algorithm to comprehensively consider the mechanical structure limitations of the truck crane, the distribution of obstacles in the operating environment, and the safety spacing requirements to divide an initial operating area. Since this initial operating area may contain some irregular geometric shapes, the system uses the convex hull decomposition algorithm to decompose it into several regular convex polygonal areas. The combination of these convex polygonal areas constitutes the final operating area, which is convenient for subsequent calculations and planning.

[0077] The boom system of a truck-mounted crane is composed of multiple joints. To accurately evaluate the operating capacity, an accurate kinematic model needs to be established. The system adopts the joint geometric parameter method, taking into account the geometric parameters such as the position of each joint of the boom, the length of the connecting rod, the rotation angle, etc., to establish a complete boom kinematic model. Based on this model, the system can calculate the working state of the boom in different positions and postures, and then analyze the safe load capacity of each position point in the final operating area. The calculation of this load capacity takes into account the mechanical strength, balance stability and force conditions of each component of the boom.

[0078] After obtaining the safe load capacity distribution in the operating area, the system divides the operating area into safety levels. This division process comprehensively considers multiple factors such as the safe load capacity of each location point, the distance from obstacles, and the difficulty of operation. By setting different safety level thresholds, the operating area is divided into multiple safety levels to form an operating space safety level model. Areas with high safety levels have larger load margins and operating space, which are suitable for heavy-load operations; areas with low safety levels may be subject to load restrictions or obstacles, and require more cautious operation planning. This classification model provides an important decision-making basis for subsequent operation planning, which can help the system select the optimal operation plan and ensure the safety and efficiency of the operation process.

[0079] The entire process realizes the transformation from environmental perception data to work space planning. Through scientific algorithms and model construction, the complex work environment is transformed into a clear safety level distribution, providing a reliable space planning foundation for the intelligent operation of the truck crane. This space division method based on safety level can effectively prevent safety risks in the operation process and improve work efficiency, which is of great significance to improving the intelligent level of the truck crane.

[0080] In this embodiment, the regional boundary generation algorithm and the convex hull decomposition method are used to ensure that the planned working area is scientific and reasonable, and to exclude inaccessible and dangerous areas; the boom kinematic model is established based on the joint geometric parameter method to accurately calculate the safe load capacity distribution in the working area; through the hierarchical analysis of the safe load capacity distribution, a working space safety level model is constructed to assign safety weights to different areas; the regional division is optimized in combination with the obstacle status data to effectively avoid potential collision risks and improve planning reliability; through detailed regional division and load analysis, a balance between working efficiency and working safety is achieved.

[0081] S103. Based on the work space safety level model and the obstacle status data, a fast random tree connection method combined with dynamic programming is used to generate candidate obstacle avoidance paths that meet the load constraints; the optimal obstacle avoidance path is selected according to the comprehensive score calculated based on the path safety and load margin, and the optimal obstacle avoidance path is converted into an obstacle avoidance trajectory and control instructions through a quintic polynomial interpolation algorithm; a real-time distance field algorithm is used to monitor the distance between the boom and the obstacle, and prompts, warnings and control instructions are executed based on the preset hierarchical control threshold.

[0082] After obtaining the working space safety level model and obstacle status data, the system needs to plan a safe and effective obstacle avoidance path. The system uses a fast random tree connection method as the basic path planning algorithm. This method can quickly explore feasible paths in complex environments by randomly sampling in space and establishing a tree structure. At the same time, combined with the dynamic programming method, load constraints are introduced into the path search process to ensure that the generated path meets the load capacity requirements of each location point. Through this combined method, the system can generate multiple candidate obstacle avoidance paths that meet safety requirements. These paths can ensure that the boom avoids obstacles during movement and meets load constraints.

[0083] In order to select the best path from multiple candidate paths, the system has established a comprehensive evaluation mechanism. The evaluation indicators mainly include path safety and load margin. Path safety reflects the level of safe distance between the path and obstacles, while load margin indicates the difference between the actual load and the maximum allowable load at each point on the path. The system performs weighted calculation on these two indicators to obtain a comprehensive score for each candidate path. The path with the highest comprehensive score is selected as the final obstacle avoidance path. In order to convert this path into a smooth motion trajectory, the system uses a quintic polynomial interpolation algorithm for processing. This high-order polynomial interpolation can ensure the continuity of the path in position, velocity and acceleration, and the generated trajectory is smoother, which is convenient for the crane to execute. The system converts the smoothed trajectory into specific execution instructions, including the motion parameters and timing arrangements of each joint.

[0084] During the process of the truck crane executing the obstacle avoidance trajectory, the system needs to monitor the distance between the boom and the obstacle in real time to ensure the safety of the movement process. The system uses a real-time distance field algorithm for distance monitoring, which can quickly calculate the shortest distance between each part of the boom and the surrounding obstacles. According to the calculated distance value, the system processes it according to the pre-set hierarchical control threshold. When the distance is far, the system maintains normal operation; when the distance is close to the warning line, the system issues an early warning prompt; when the distance is too close and a collision may occur, the system immediately executes control instructions such as deceleration or stop. This hierarchical control mechanism can effectively prevent collision accidents while ensuring the continuity of operations.

[0085] The entire obstacle avoidance process forms a closed-loop intelligent control system. From path planning to trajectory generation, to real-time monitoring and safety control, each link cooperates with each other to ensure that the truck crane can complete the obstacle avoidance operation safely and smoothly. This control solution based on multi-level safety assurance not only ensures operating efficiency, but also provides reliable safety assurance, reflecting the advancement and practicality of the truck crane's intelligent control system.

[0086] In this embodiment, the intelligent obstacle avoidance function of the truck crane in complex environments is realized through multi-level path planning and safety control mechanisms. The fast random tree connection method combined with dynamic programming can efficiently generate multiple candidate paths that meet the load requirements, avoiding the limitations that may be caused by traditional single path planning. Through the comprehensive scoring mechanism of path safety and load margin, the optimal path can be scientifically selected, which improves the reliability of path planning; ensures the smoothness and continuity of the obstacle avoidance trajectory, reduces mechanical shock, and extends the service life of the equipment. The real-time distance field algorithm cooperates with the hierarchical control mechanism to form a progressive safety assurance system from prompts to forced control, which not only ensures the continuity of operations, but also can prevent and stop possible collision accidents in time; it significantly improves the safety and efficiency of truck crane operations, reduces the difficulty of operation, and reduces the risk of human errors. At the same time, it has good practicality and reliability, and provides effective technical support for the intelligent upgrade of truck cranes.

[0087] In an optional implementation, after the point cloud data is subjected to noise reduction and point cloud registration processing by using a bilateral filtering algorithm, a scene map is obtained by using a dynamic positioning mapping algorithm; the truck crane attitude data is processed by using a quaternion attitude solution method, and the scene map is subjected to posture correction to obtain an environmental correction model, which includes:

[0088] Each sampling point in the point cloud data is sequentially used as a point to be processed, and the neighboring points of the point to be processed are searched within a preset radius, and the spatial distance weight and the intensity value weight of the point to be processed and the corresponding neighboring points are respectively calculated, and the product of the spatial distance weight and the intensity value weight is used as the comprehensive weight, and the position of the point to be processed is updated based on the comprehensive weight to obtain the filtered point cloud data;

[0089] Calculate the normal vector and principal curvature value of each point in the filtered point cloud data, select the local extreme point of the principal curvature value as the feature point, construct the local descriptor of the feature point, perform feature matching on the local descriptor through fast neighbor search, remove the wrong matching points in combination with the random sampling consistency algorithm, determine the retained matching points, calculate the pose transformation matrix based on the retained matching points, and obtain the point cloud registration data;

[0090] Determine the initial pose based on the pose transformation matrix, extract the local plane features in the point cloud registration data to construct a pose graph, calculate the point cloud distance field value between consecutive frames, compare the point cloud distance field value with the adaptive threshold to identify the dynamic point, assign attenuation weights to the local plane features corresponding to the dynamic point, construct a weighted pose optimization equation, solve it to obtain the optimized pose, transform the point cloud of the current frame to the global coordinate system through the optimized pose, and update the scene map;

[0091] Based on the optimized posture and the point cloud motion information in the scene map, a quaternion equation describing the spatial posture is constructed, the quaternion equation is numerically integrated to obtain a predicted posture, and an error state equation is constructed based on the deviation between the predicted posture and the actual observed posture and solved to obtain a posture quaternion;

[0092] Calculate each element of the rotation matrix according to the posture quaternion, perform coordinate transformation on the point cloud position and normal vector in the scene map through the rotation matrix, update local plane parameters based on the transformed point cloud position and normal vector, reconstruct the spatial position relationship of the feature points, and generate an environment correction model.

[0093] The point cloud distance field value is specifically a metric that describes the distance between any point in the point cloud data and its nearest point. In three-dimensional space, for each point in the point cloud, a continuous distance field distribution can be obtained by calculating the distance from the point to other surrounding points. This distance field value reflects the density distribution characteristics of the point cloud and can be used to measure the similarity and spatial distribution changes between point clouds. In a dynamic environment, changes in the distance field value can help identify moving objects, because moving objects will cause significant changes in the point cloud distribution in the local area, resulting in significant differences in the distance field value.

[0094] The quaternion specifically refers to a mathematical tool for describing rotation in three-dimensional space, which consists of a real part and three imaginary parts. Compared with the Euler angle representation method, the quaternion avoids the universal joint deadlock problem and can achieve smooth and continuous attitude representation. In the spatial attitude description, the real part of the quaternion represents the rotation angle information, and the three imaginary parts represent the direction information of the rotation axis. Continuous interpolation operations can be performed between quaternions, which gives it a unique advantage in smooth attitude transformation and trajectory planning. Through quaternion operations, coordinate transformation and attitude prediction of spatial position can be easily realized, providing a mathematical basis for real-time attitude control.

[0095] In a specific implementation, first obtain the point cloud data scanned by the three-dimensional laser radar mounted on the truck crane, and pre-process the point cloud data. Take each point in the point cloud data as the center point to be processed, set a spherical area with a fixed radius around it, and search for all neighboring points falling within this area. Calculate the spatial distance from the center point to each neighboring point. The closer the distance, the greater the weight assigned. At the same time, compare the intensity value difference between the center point and each neighboring point. The closer the intensity value, the greater the weight assigned. Multiply the spatial distance weight and the intensity value weight to obtain a comprehensive weight. Use the weighted average of the coordinates of all neighboring points, with the weight being the respective comprehensive weights, to obtain the new coordinates of the center point. After all points are processed in this way, the filtered point cloud data is obtained, and the noise points are effectively removed. For example, the coordinates of the center point are (1, 2, 3), and the intensity value is 0.8, and the coordinates of its neighboring points are (1.1, 2.1, 3.1), and the intensity value is 0.7; (0.9, 1.9, 2.9), and the intensity value is 0.9. The spatial distance weight and the intensity value weight are calculated and multiplied to obtain the comprehensive weight, and finally the new coordinates of the center point are calculated.

[0096] Next, point cloud registration is performed. Calculate the normal vector and principal curvature value of each point in the filtered point cloud data. Select the point with the local maximum principal curvature value as the feature point. Construct a local descriptor for each feature point to describe the geometric features around the point. Use the fast nearest neighbor search algorithm to find matching feature point pairs in the feature point sets of two adjacent frames of point cloud data. Since there will inevitably be some wrong matches, the random sampling consistency algorithm is used to eliminate these wrong matches and finally determine reliable matching point pairs. Based on these reliable matching point pairs, calculate the pose transformation matrix between the two frames of point cloud data. For example, there are 1000 feature points in each of the two frames of point cloud data. Through the fast nearest neighbor search and random sampling consistency algorithms, 500 pairs of reliable matching points are finally determined, and the pose transformation matrix is ​​calculated based on these matching points.

[0097] Then, dynamic positioning and mapping are performed to construct a scene map. The pose transformation matrix obtained by point cloud registration is used as the initial pose. Local plane features in the point cloud data are extracted to construct a pose graph. The point cloud distance field value between two consecutive frames of point cloud data is calculated. The calculated point cloud distance field value is compared with an adaptive threshold. If it is greater than the threshold, the point is considered to be a dynamic point. An attenuation weight is assigned to the local plane feature corresponding to the dynamic point to reduce its influence on pose optimization. A weighted pose optimization equation is constructed and solved to obtain the optimized pose. The point cloud data of the current frame is transformed to the global coordinate system through the optimized pose, and the scene map is updated. For example, if the point cloud distance field value between two consecutive frames of point cloud data is calculated to be 0.2 meters and the adaptive threshold is 0.1 meters, the point is considered to be a dynamic point, and an attenuation weight of 0.5 is assigned to the corresponding local plane feature.

[0098] The posture data of the truck-mounted crane is obtained and processed using the quaternion posture solution method. Based on the optimized posture and the point cloud motion information in the scene map, the quaternion equation describing the spatial posture is constructed. The quaternion equation is numerically integrated to obtain the predicted posture. The deviation between the predicted posture and the actual observed posture is calculated, and the error state equation is constructed and solved to obtain the accurate posture quaternion.

[0099] Finally, perform environmental correction and generate an environmental correction model. Calculate each element of the rotation matrix based on the obtained pose quaternion. Use the rotation matrix to transform the coordinates of the point cloud position and normal vector in the scene map. Based on the transformed point cloud position and normal vector, update the local plane parameters, reconstruct the spatial position relationship of the feature points, and generate an environmental correction model. For example, if the calculated rotation matrix is ​​[[0.8, -0.6, 0], [0.6, 0.8, 0], [0, 0, 1]], then multiply the coordinates and normal vectors of all points in the scene map by the rotation matrix for coordinate transformation.

[0100] In this embodiment, through the steps of point cloud filtering, registration and dynamic point removal, the influence of noise and dynamic targets on the scene model is effectively reduced, and the accuracy of the model is improved; the quaternion attitude solution method and posture correction technology are used to effectively compensate for the influence of the posture change of the truck crane on the scene model, and the stability of the model is enhanced; the optimization strategies such as fast nearest neighbor search and random sampling consistency algorithm are used to improve the efficiency of point cloud registration and posture estimation, thereby improving the overall modeling efficiency.

[0101] In an optional implementation, the environment correction model and the obstacle distance data are spatially fused by iterative closest point registration to construct a dynamic environment map; obstacle features in the dynamic environment map are identified by combining a target detection algorithm with a Kalman filter, and the obstacle status data obtained includes:

[0102] Taking the feature point set in the environmental correction model as the first feature point set, taking the feature point set in the obstacle distance data as the second feature point set, taking each feature point in the first feature point set as a reference feature point, searching for the nearest neighbor point in the second feature point set, and constructing a point pair set with the reference feature point and the nearest neighbor point;

[0103] Calculate the centroid coordinates based on the point pair set, calculate the relative position of each point in the point pair set relative to the centroid coordinates, construct a covariance matrix of the relative positions, and perform singular value decomposition on the covariance matrix to obtain a rotation matrix and a translation vector;

[0104] Based on the rotation matrix and the translation vector, coordinate transformation is performed on the reference feature point, and the nearest neighbor point search, centroid coordinate calculation and coordinate transformation are repeatedly performed until the position change of the reference feature point is less than a preset threshold value, thereby obtaining a dynamic environment map;

[0105] Clustering and segmenting the point cloud in the dynamic environment map, extracting the position center, main direction vector and size parameter of each cluster, constructing a feature vector, and establishing an obstacle state vector including a position vector, a velocity vector and an acceleration vector based on the feature vector;

[0106] A state transfer equation is constructed according to the obstacle state vector at adjacent moments, and an observation equation is constructed by taking the characteristic vector as an observation value. Based on the state transfer equation and the observation equation, a deviation between a predicted value and an observed value of the obstacle state vector is calculated, and the obstacle state vector is updated using the deviation to obtain obstacle state data.

[0107] In a specific implementation, first, obtain the environment correction model and obstacle distance data. The environment correction model can be obtained by SLAM (simultaneous localization and mapping) system, 3D reconstruction technology and other methods, providing a preliminary understanding of the surrounding environment, including a set of feature points in the environment, such as corner points, plane features, etc., and the coordinates of these feature points in three-dimensional space. Obstacle distance data can be obtained through sensors such as lidar and depth cameras, which provide measurement information on obstacle distances and can also be converted into a set of feature points, including feature points on the obstacle surface and their spatial coordinates.

[0108] Assume that the feature point set of the environmental correction model includes points A(1, 2, 3), B(4, 5, 6), and C(7, 8, 9). The feature point set of the obstacle distance data includes points a(1.1, 2.2, 3.3), b(4.2, 5.1, 6.4), and c(6.9, 7.8, 8.7).

[0109] Next, perform iterative closest point (ICP) registration. Take each feature point in the environmental correction model as the reference feature point. For example, take point A as the reference and search for the point closest to A in the feature point set of the obstacle distance data. Assuming that point a is closest to A, A and a form a point pair. Perform this operation for each feature point in the environmental correction model to obtain a point pair set {(A, a), (B, b), (C, c)}.

[0110] Then, the centroid coordinates are calculated based on the point pair set. The coordinates of all points in the point pair set are summed up and divided by the number of points to obtain the centroid coordinates. For example, the centroid of the x coordinate is (1+1.1+4+4.2+7+6.9) / 6=4.03. Similarly, the centroid of the y coordinate and the z coordinate can be calculated. Assume that the calculated centroid coordinates are (4.03, 5.05, 6.22).

[0111] Next, calculate the relative position of each point relative to the centroid coordinates and construct the covariance matrix. For example, the relative position of point A relative to the centroid is (-3.03, -3.05, -3.22). Calculate the relative position of all points relative to the centroid and then construct the covariance matrix.

[0112] Perform singular value decomposition on the covariance matrix to obtain the rotation matrix and translation vector. The rotation matrix represents the angle that the environmental correction model needs to rotate, and the translation vector represents the distance that the environmental correction model needs to translate.

[0113] Based on the rotation matrix and translation vector, the coordinates of the feature points in the environmental correction model are transformed. For example, the rotation matrix and translation vector are applied to point A to obtain the new coordinate A'.

[0114] Repeat the steps of searching for the nearest neighbor, calculating the centroid coordinates, and coordinate transformation until the position change of the reference feature point is less than the preset threshold (for example, 0.01). Assume that after multiple iterations, the feature point set of the environmental correction model becomes A'(1.05, 2.1, 3.2), B'(4.1, 5.05, 6.3), C'(7.1, 7.9, 8.8). This is a dynamic environment map that incorporates obstacle distance data.

[0115] Cluster the point cloud in the dynamic environment map and divide the points that are close to each other into the same cluster. Extract the center of each cluster, the main direction vector and the size parameter to construct the feature vector. For example, the center of a cluster is (2, 3, 4), the main direction vector is (0, 1, 0), and the size parameter is (1, 2, 1).

[0116] Based on the feature vector, an obstacle state vector including a position vector, a velocity vector, and an acceleration vector is established. For example, the position vector is (2, 3, 4), and the initial velocity and acceleration are set to (0, 0, 0).

[0117] According to the obstacle state vectors at adjacent moments, the state transfer equation is constructed, and the characteristic vector is used as the observation value to construct the observation equation. Using Kalman filtering, the deviation between the predicted value and the observed value of the obstacle state vector is calculated, and the obstacle state vector is updated using the deviation to obtain the obstacle state data, such as the position vector (2.1, 3.1, 4.1), the velocity vector (0.1, 0.1, 0.1), and the acceleration vector (0, 0, 0).

[0118] In this embodiment, by fusing the environment correction model and the real-time obstacle distance data, the errors in the environment model can be effectively corrected, the accuracy of the environment map can be improved, and the map can be closer to the real environment; the obstacle information can be integrated into the environment map in real time to construct a dynamic environment map, so as to better reflect the changes in the environment, such as moving obstacles; by analyzing the dynamic environment map, the position, speed, acceleration and other status information of the obstacle can be accurately identified, providing reliable data support for subsequent path planning, obstacle avoidance and other tasks.

[0119] In an optional implementation, based on the dynamic environment map and in combination with the obstacle status data, dividing the initial operation area by using an area boundary generation algorithm includes:

[0120] Calculate the outer polygon of the point cloud data in the dynamic environment map, select three non-collinear feature points within the scope of the outer polygon to construct a triangular mesh, insert the remaining point cloud data into the triangular mesh in sequence, iteratively adjust the triangular mesh by maximizing the minimum inner angle of the triangle, and if the minimum inner angle of the triangle after adjustment is greater than the minimum inner angle of the triangle before adjustment, update the connection relationship of the triangular mesh until all the point cloud data are added to the triangular mesh;

[0121] In the triangular mesh, for each triangle, mark it as the current triangle, determine whether there is a circle with a preset radius that passes through the three vertices of the current triangle, and the interior of the circle does not contain any point in the point cloud data. If so, the current triangle is a valid triangle; extract the edge segments of all the valid triangles, determine the edge segment set, take the segment that appears once in the edge segment set as the target boundary segment, and reorganize the target boundary segment into a closed area boundary curve in a way that the endpoints are connected;

[0122] Based on the position vector and velocity vector in the obstacle state data, the movement trajectory of the obstacle is predicted within a preset time window, and the expansion area containing the current position and movement trajectory of the obstacle is calculated as the obstacle influence area. The area enclosed by the area boundary curve is superimposed with the obstacle influence area to obtain the initial operation area.

[0123] In a specific implementation, first, the point cloud data of the dynamic environment is obtained, and the outer polygon of the point cloud data is calculated to determine the overall range of the point cloud data. For example, the outer polygon can be calculated using a convex hull algorithm. Assume that the point cloud data contains the following point coordinates: (1, 2), (2, 4), (3, 1), (5, 3), (4, 5), and the outer polygon vertices calculated by the convex hull algorithm are (1, 2), (2, 4), (4, 5), (5, 3), (3, 1).

[0124] Within the scope of the outer polygon, select three non-collinear feature points, such as three vertices on the outer polygon, to construct an initial triangular mesh. Taking the above point cloud data as an example, three points (1, 2), (2, 4), and (5, 3) can be selected to construct an initial triangular mesh.

[0125] Insert the remaining point cloud data into the triangular mesh in sequence. For each point to be inserted, find the triangle that contains the point and has the largest minimum internal angle, and connect the point to the three vertices of the triangle to form three new triangles. For example, insert the point (3, 1) into the triangle formed by (1, 2), (2, 4), and (5, 3) to form three new triangles: (1, 2) (2, 4) (3, 1), (1, 2) (3, 1) (5, 3), (2, 4) (3, 1) (5, 3).

[0126] Iterate the triangle mesh. For each triangle, calculate the size of its three internal angles and find the minimum internal angle. Then, try to swap the connection relationship of any edge of the triangle to form a new triangle. If the minimum internal angle of the new triangle is greater than the minimum internal angle of the original triangle, update the connection relationship of the triangle mesh. Repeat this process until the minimum internal angle of all triangles no longer increases.

[0127] In the adjusted triangle mesh, filter out valid triangles. For each triangle, determine whether there is a circle with a preset radius (for example, a radius of 1) that passes through the three vertices of the triangle and does not contain any point cloud data points. If such a circle exists, the triangle is a valid triangle.

[0128] Extract the edge segments of all valid triangles and determine the edge segment set. The segments that appear only once are used as target boundary segments. For example, assuming that the valid triangles are: (1, 2)(2, 4)(3, 1) and (1, 2)(3, 1)(5, 3), then the edge segment set is (1, 2)(2, 4), (2, 4)(3, 1), (3, 1)(1, 2), (1, 2)(3, 1), (3, 1)(5, 3), (5, 3)(1, 2). Among them, (2, 4)(3, 1) and (3, 1)(5, 3) appear only once, so they are the target boundary segments.

[0129] The target boundary line segments are reorganized in a way that the endpoints are connected to form a closed region boundary curve. For example, the line segments (2, 4) (3, 1) and (3, 1) (5, 3) are connected to form a region boundary curve.

[0130] Obtain obstacle status data, including position vector and velocity vector. For example, assume that the obstacle's position vector is (6, 6) and its velocity vector is (1, 1).

[0131] The movement trajectory of the obstacle is predicted within a preset time window (e.g., the time window is 5 seconds). For example, based on the current position and speed of the obstacle, its trajectory within the next 5 seconds can be predicted to be a straight line.

[0132] Calculate the expansion area that includes the current position and motion trajectory of the obstacle as the obstacle influence area. For example, you can use the obstacle trajectory as the center line and expand a certain distance to both sides to form a rectangle or polygon as the obstacle influence area.

[0133] The area enclosed by the area boundary curve is superimposed on the obstacle influence area to obtain the final initial operation area. For example, the intersection of the polygon enclosed by the area boundary curve and the polygon of the obstacle influence area can be calculated to obtain the final initial operation area.

[0134] In this embodiment, through the boundary generation algorithm based on triangular meshes, it is possible to more accurately fit the actual contour of the environment, avoiding the errors caused by the traditional methods based on rectangular or regular polygonal division; it is possible to adjust the boundaries of the working area in real time according to changes in the dynamic environment, so as to better adapt to changes in the environment; by considering the movement trajectory of obstacles, it is possible to effectively avoid collisions between the working area and obstacles, thereby improving the safety of operations.

[0135] In an optional implementation, using a convex hull decomposition algorithm, determining the final operating area includes:

[0136] Connect any two convex vertices in the initial operation area, check whether the line between the two convex vertices has an intersection with the area boundary curve, and construct the lines without intersections into a connected graph;

[0137] Selecting a concave vertex with the smallest inner angle in the initial operation area as a segmentation starting point, selecting an optimal segmentation line in the connected graph to divide the area into a first sub-area and a second sub-area, performing segmentation steps on the first sub-area and the second sub-area respectively until each sub-area after segmentation becomes a convex polygon, obtaining a convex polygon sub-area, and forming all the convex polygon sub-areas into an area set;

[0138] Calculate the area change of adjacent convex polygonal sub-regions with common edges in the region set after merging. When the merged region remains a convex polygon and the area change is less than a preset change threshold, merge the adjacent convex polygonal sub-regions into a new sub-region. Repeat the merging until there are no sub-regions that can be merged, and obtain the final working area.

[0139] The convex vertex specifically refers to a vertex on the polygon outline whose internal angle is less than 180 degrees. From the perspective of geometric characteristics, the convex vertex protrudes outward, and when any ray is drawn from the vertex to the inside, the ray will only intersect the polygon boundary once. The characteristic of a convex vertex is that in any small fan-shaped area centered on the vertex, the part located inside the polygon constitutes a continuous area.

[0140] The concave vertex specifically refers to a vertex on the polygon outline whose internal angle is greater than 180 degrees. From the perspective of geometric characteristics, a concave vertex is concave inward, and when a ray is drawn from the vertex in certain directions, the ray may intersect the polygon boundary multiple times. The characteristic of a concave vertex is that in certain fan-shaped areas centered on the vertex, the part located inside the polygon will form a discontinuous area. The existence of a concave vertex makes the polygon a non-convex polygon, which often increases the complexity of area division and path planning.

[0141] In a specific implementation, first, the boundary information of the initial operation area is obtained, such as a series of vertex coordinates arranged in sequence. According to these vertex coordinates, it is determined whether each vertex is a convex vertex or a concave vertex. The inner angle of a convex vertex is less than 180 degrees, and the inner angle of a concave vertex is greater than 180 degrees.

[0142] Next, build the connectivity graph. Traverse all convex vertex pairs in the initial working area and connect each pair of convex vertices. Then, check whether each link has an intersection with the region boundary curve. If a link does not intersect the boundary curve, add the link to the connectivity graph.

[0143] Select the starting point for segmentation. Among all the concave vertices in the initial working area, select the concave vertex with the smallest inner angle as the starting point for segmentation.

[0144] Select the best segmentation line. In the connectivity graph, find the line connected to the segmentation starting point. For each line, calculate the area change after the initial operation area is divided into two sub-areas. Select the line that minimizes the area change as the best segmentation line.

[0145] Recursive segmentation. Use the best segmentation line to segment the initial working area into the first sub-area and the second sub-area. Repeat the steps of selecting the segmentation starting point, selecting the best segmentation line, and recursive segmentation for the first sub-area and the second sub-area respectively until all sub-areas become convex polygons. All convex polygon sub-areas are grouped into a region set.

[0146] Merge sub-regions. Traverse all pairs of adjacent convex polygon sub-regions with common edges in the region set. For each pair of adjacent sub-regions, calculate the area change after merging and the shape of the merged region. If the merged region is still a convex polygon and the area change is less than the preset change threshold, merge the two adjacent sub-regions into a new sub-region. Repeat the merging operation until there are no sub-regions that can be merged. The final region set is the final operation region.

[0147] For example, a pentagonal work area has vertex coordinates of (0, 0), (1, 0), (2, 1), (1, 2), and (0, 1). Among them, (2, 1) is a concave vertex. Connect (0, 0) and (1, 2) to get a dividing line. Use this dividing line to divide the pentagon into a triangle and a quadrilateral. The triangle is already a convex polygon. The quadrilateral is further divided into two triangles using (1, 0) and (1, 2) as dividing lines. Finally, three triangles are obtained. Assuming that the preset area change threshold is 0.5, if after merging any two adjacent triangles, the new polygon is still a convex polygon and the area change is less than 0.5, then they are merged. Finally, the final work area composed of one or more convex polygons is obtained.

[0148] In this embodiment, decomposing a complex non-convex polygonal working area into several convex polygonal sub-areas can simplify subsequent operations on the area, such as path planning, coverage planning, etc., because convex polygons have simpler geometric properties and are easier to calculate and process; through the merging operation, the number of convex polygons in the final working area can be reduced, thereby improving the computational efficiency of subsequent operations; by controlling the area change threshold, the complexity and accuracy of the final working area can be balanced, thereby improving the quality of the final result.

[0149] In an optional implementation, based on the workspace safety level model and the obstacle state data, using a fast random tree connection method combined with dynamic programming to generate an obstacle avoidance candidate path that meets the load constraint includes:

[0150] Establishing a state space of the truck crane, the state space including configuration parameters and motion parameters of the truck crane, and establishing a state transfer cost function based on the state space, the state transfer cost function including a distance cost between state points, a path curvature cost, and a safety level cost based on a work space safety level model;

[0151] In the state space, determine a starting state point and a target state point, respectively establish bidirectional random trees with the starting state point and the target state point as root nodes, including a forward random tree and a reverse random tree, and randomly sample in the state space to obtain candidate state points;

[0152] Taking the forward random tree or the reverse random tree as the current random tree, determining the tree node closest to the candidate state point in the current random tree, and extending a preset distance from the tree node toward the candidate state point to obtain an extended node;

[0153] Based on the workspace safety level model, determine the safety level of the extension node, and based on the obstacle status data, determine whether the path from the tree node to the extension node intersects with an obstacle, and when the safety level of the extension node meets the preset area and does not intersect with the obstacle, add the extension node to the current random tree;

[0154] Determine whether there is a connection path between the bidirectional random trees, generate an initial obstacle avoidance path when there is a connection path, sample at the path nodes of the initial obstacle avoidance path to obtain a set of candidate nodes, calculate the path cost of each node in the set of candidate nodes based on the state transition cost function; and generate a set of obstacle avoidance candidate paths that meet the load constraint based on the path cost using a dynamic programming method.

[0155] In a specific implementation, first, a state space of the truck crane is established. The state space contains configuration parameters describing the state of the truck crane, such as the length of the telescopic arm, the rotation angle, etc., and motion parameters, such as position, speed, etc. For example, the state of the truck crane can be represented by a six-dimensional vector: (x, y, z, α, β, γ), where (x, y, z) represents the position of the end effector of the truck crane in three-dimensional space, and (α, β, γ) represent the length, rotation angle and pitch angle of the telescopic arm, respectively.

[0156] Then, a state transition cost function is established based on the state space. This function is used to evaluate the cost of transferring from one state point to another, which contains three components: distance cost, path curvature cost, and safety level cost. The distance cost can be expressed by the Euclidean distance between two state points. The path curvature cost can be expressed by the curvature of the path between two state points. The greater the curvature, the higher the cost. The safety level cost is based on a predefined working space safety level model. The lower the safety level, the higher the cost. For example, the area close to the high-voltage line has a low safety level and a high cost, while the open area has a high safety level and a low cost.

[0157] Next, determine the starting state point and the target state point in the state space, and establish a forward random tree and a reverse random tree with them as root nodes respectively. Taking the forward random tree as an example, randomly sample a candidate state point in the state space, such as (10, 5, 2, 30, 45, 60). Find the tree node closest to the candidate state point in the forward random tree, such as (8, 4, 1, 25, 40, 55). Extend the preset distance from the tree node to the candidate state point, for example, the extended distance is 1, and the extended node (9, 4.5, 1.5, 27.5, 42.5, 57.5) is obtained.

[0158] Subsequently, the safety and feasibility of the extended node are determined. Based on the workspace safety level model, the safety level of the extended node is determined, and based on the obstacle status data, whether the path from the tree node to the extended node intersects with the obstacle is determined. Assuming that the safety level of the extended node meets the preset area and does not intersect with the obstacle, the extended node is added to the current random tree. Repeat the above steps until the forward random tree and the reverse random tree meet, that is, there is a connection path. Assume that the nodes on the connection path are A, B, C, D, and E, where A is the starting node and E is the target node.

[0159] Then, the connection path is optimized. At each node of the connection path, such as node B, a set of candidate nodes is sampled, such as B1, B2, and B3. The path cost of each node in the candidate node set is calculated based on the state transition cost function, such as the path cost of A-B1-C, the path cost of A-B2-C, and the path cost of A-B3-C. Using the dynamic programming method, the node with the smallest path cost, such as B1, is selected to replace the original path node B to obtain a new path A-B1-CDE. Similar operations are performed on all nodes on the connection path, and finally a set of obstacle avoidance candidate paths that meet the load constraints is generated. For example, the generated candidate path set contains three paths: Path 1: A-B1-C1-D1-E, Path 2: A-B2-C2-D2-E, Path 3: A-B3-C3-D3-E.

[0160] In this embodiment, a bidirectional fast random tree search strategy is adopted, which can quickly find a feasible obstacle avoidance path in a complex environment, which is more efficient than the traditional one-way search method; based on the working space safety level model, the safety level cost is integrated into the state transfer cost function, which can effectively guide the path planning algorithm to generate a safer obstacle avoidance path and avoid dangerous areas; combined with the dynamic programming method to optimize the initial path, it can effectively reduce the length and curvature of the path, and generate a smoother and more optimized obstacle avoidance path, thereby improving the operating efficiency and safety of the truck crane.

[0161] In an optional implementation, selecting an optimal obstacle avoidance path according to a comprehensive score calculated based on path safety and load margin, and converting the optimal obstacle avoidance path into an obstacle avoidance trajectory and a control instruction by a quintic polynomial interpolation algorithm includes:

[0162] For each candidate path in the candidate path set, a discrete point sequence on the candidate path is calculated, and for each discrete point in the discrete point sequence, a distance from the discrete point to the nearest obstacle is calculated, and a ratio of the distance to a preset safety distance threshold is used as the path safety;

[0163] For each discrete point in the discrete point sequence, a ratio of the rated lifting weight to the actual lifting weight of the discrete point is calculated as a safety factor, and a ratio of the safety factor to a preset reference safety factor is used as a load margin;

[0164] Performing a weighted combination of the path safety and the load margin to obtain a path comprehensive score, and selecting a candidate path with the highest path comprehensive score as the optimal obstacle avoidance path;

[0165] Selecting two adjacent path nodes on the optimal obstacle avoidance path, and establishing a quintic polynomial trajectory equation based on position constraints, velocity constraints, and acceleration constraints of the two adjacent path nodes;

[0166] The quintic polynomial trajectory equation is sampled to obtain the position sequence, velocity sequence and acceleration sequence of the end effector in Cartesian space; the position sequence is transformed through inverse kinematics to obtain the angle sequence in the joint space, and the angle sequence is numerically differentiated to obtain the velocity sequence in the joint space; the angle sequence and the velocity sequence are converted into a motor speed sequence and a cylinder extension and retraction speed sequence to generate corresponding control instructions.

[0167] In a specific implementation, first, a working environment model of the robot arm is constructed, including obstacle information and the range of motion of the robot arm itself. Obstacle information can be obtained through sensors, such as depth cameras, laser radars, etc., and represented as geometric shapes, such as cuboids, spheres, etc. The range of motion of the robot arm is determined by its joint structure and motion restrictions.

[0168] Next, based on the constructed environment model, a path planning algorithm is used to generate a series of candidate paths from the starting point to the target point. These candidate paths are represented by a series of discrete path nodes.

[0169] For each candidate path, evaluate its safety and load capacity. First, calculate the distance from each discrete point on the path to the nearest obstacle. This distance can be obtained by calculating the minimum distance from the point to the geometric shape. Compare this distance with the preset safety distance threshold, for example, divide the two to obtain the path safety. The safety distance threshold is set according to the actual application scenario. For example, it can be set to 5% of the maximum reach of the robot arm. Assuming that the distance from a discrete point to the nearest obstacle is 0.5 meters and the safety distance threshold is 0.1 meters, the path safety of this point is 5.

[0170] Then, calculate the load margin for each discrete point. The load margin reflects the load-bearing capacity of the robot at that point. It can be obtained by calculating the ratio of the rated lifting weight to the actual lifting weight at that point, and then comparing it with the preset reference safety factor. The reference safety factor is usually set between 1.2 and 1.5. For example, assuming that the rated lifting weight of a discrete point is 10 kg, the actual lifting weight is 5 kg, and the reference safety factor is 1.2, then the safety factor of the point is 2 and the load margin is 1.67.

[0171] The path safety and load margin are weighted together to obtain the path comprehensive score. The weight coefficient can be adjusted according to the actual application scenario. For example, if safety is more important, a higher weight can be given to the path safety. Assuming that the weight of the path safety is 0.7 and the weight of the load margin is 0.3, the comprehensive score of the discrete point is 0.7×5+0.3×1.67=4.001. The comprehensive scores of all discrete points on the path are averaged to obtain the comprehensive score of the candidate path.

[0172] Among all candidate paths, the path with the highest comprehensive score is selected as the optimal obstacle avoidance path.

[0173] After determining the optimal obstacle avoidance path, it needs to be converted into executable trajectories and control instructions. Select two adjacent path nodes on the optimal obstacle avoidance path, such as node A and node B. Construct a fifth-order polynomial trajectory equation based on the position, velocity, and acceleration constraints of these two nodes. The position constraint refers to the coordinates of node A and node B; the velocity constraint refers to the velocity of the robot at node A and node B, for example, it can be set to zero; the acceleration constraint refers to the acceleration of the robot at node A and node B, for example, it can also be set to zero.

[0174] By solving the fifth-order polynomial trajectory equation, the position, velocity and acceleration sequence of the robot end effector in Cartesian space can be obtained. For example, it can be sampled every 0.01 seconds.

[0175] The position sequence in Cartesian space is converted into the angle sequence in joint space through inverse kinematics. Inverse kinematics is the process of converting the position of the end effector into the joint angle. The angle sequence is numerically differentiated to obtain the velocity sequence in joint space.

[0176] Finally, the angle sequence and speed sequence of the joint space are converted into the motor speed sequence and the cylinder extension speed sequence to generate the corresponding control instructions. These control instructions can be directly sent to the controller of the robot arm to control the robot arm to move along the planned trajectory.

[0177] In this embodiment, by comprehensively considering the path safety and load margin and selecting the optimal obstacle avoidance path, the collision between the robot arm and obstacles can be effectively avoided, thereby improving the safety of the operation; by generating a smooth obstacle avoidance trajectory through the quintic polynomial interpolation algorithm, the sudden stop and turn of the robot arm can be avoided, thereby improving the movement efficiency; the safety distance threshold, reference safety factor and weight factor can be adjusted according to different environments and task requirements to adapt to different application scenarios.

[0178] Figure 2 FIG. 1 is a schematic diagram of a structure of an intelligent obstacle avoidance and operation range planning system for a manual telescopic arm of a truck-mounted crane according to an embodiment of the present invention. Figure 2 As shown, the system comprises:

[0179] The first unit is used to collect environmental point cloud data, attitude data of the truck crane and obstacle distance data around the truck crane; after the point cloud data is subjected to noise reduction and point cloud registration processing by using a bilateral filtering algorithm, a scene map is obtained by using a dynamic positioning mapping algorithm; the attitude data of the truck crane is processed by using a quaternion attitude solution method, and the scene map is subjected to posture correction to obtain an environmental correction model; the environmental correction model is spatially fused with the obstacle distance data by iterative closest point registration to construct a dynamic environmental map; obstacle features in the dynamic environmental map are identified by combining a target detection algorithm with a Kalman filter to obtain obstacle status data;

[0180] The second unit is used to divide the initial working area by a regional boundary generation algorithm based on the dynamic environment map and the obstacle state data, and determine the final working area by a convex hull decomposition algorithm; establish a boom kinematic model based on a joint geometric parameter method, calculate the safe load capacity distribution within the final working area, classify the final working area according to the safe load capacity distribution, and establish a working space safety level model;

[0181] The third unit is used to generate a candidate obstacle avoidance path that meets the load constraint based on the working space safety level model and the obstacle status data by using a fast random tree connection method combined with dynamic programming; select the optimal obstacle avoidance path according to the comprehensive score calculated based on the path safety and load margin, and convert the optimal obstacle avoidance path into an obstacle avoidance trajectory and control instructions through a quintic polynomial interpolation algorithm; use a real-time distance field algorithm to monitor the distance between the boom and the obstacle, and execute prompts, warnings and control instructions based on a preset hierarchical control threshold.

[0182] According to a third aspect of the embodiments of the present invention,

[0183] An electronic device is provided, comprising:

[0184] processor;

[0185] a memory for storing processor-executable instructions;

[0186] The processor is configured to call the instructions stored in the memory to execute the aforementioned method.

[0187] A fourth aspect of the embodiments of the present invention is:

[0188] A computer-readable storage medium is provided, on which computer program instructions are stored. When the computer program instructions are executed by a processor, the aforementioned method is implemented.

[0189] The present invention may be a method, an apparatus, a system and / or a computer program product. The computer program product may include a computer-readable storage medium carrying computer-readable program instructions for executing various aspects of the present invention.

[0190] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. Intelligent obstacle avoidance and operation range planning method for manual telescopic arm of truck-mounted crane, characterized in that: include: Collect environmental point cloud data around the truck-mounted crane, the truck-mounted crane posture data and obstacle distance data; After the point cloud data is subjected to denoising and point cloud registration processing by using a bilateral filtering algorithm, a scene map is obtained by using a dynamic positioning mapping algorithm; the truck crane attitude data is processed by using a quaternion attitude solution method, and the scene map is subjected to posture correction to obtain an environmental correction model; the environmental correction model is spatially fused with the obstacle distance data by iterative closest point registration to construct a dynamic environment map; obstacle features in the dynamic environment map are identified by combining a target detection algorithm with a Kalman filter to obtain obstacle status data; Based on the dynamic environment map and in combination with the obstacle status data, the initial operation area is divided by a region boundary generation algorithm, and a convex hull decomposition algorithm is used to determine the final operation area; Establishing a boom kinematics model based on a joint geometric parameter method, calculating the safe load capacity distribution within the final operating area, grading the final operating area according to the safe load capacity distribution, and establishing an operating space safety grade model; Based on the working space safety level model and the obstacle status data, a fast random tree connection method combined with dynamic programming is used to generate candidate obstacle avoidance paths that meet load constraints; the optimal obstacle avoidance path is selected according to the comprehensive score calculated based on the path safety and load margin, and the optimal obstacle avoidance path is converted into an obstacle avoidance trajectory and control instructions through a quintic polynomial interpolation algorithm; a real-time distance field algorithm is used to monitor the distance between the boom and the obstacle, and prompts, warnings and control instructions are executed based on a preset hierarchical control threshold.

2. The method according to claim 1, characterized in that After the point cloud data is subjected to noise reduction and point cloud registration processing by using a bilateral filtering algorithm, a scene map is obtained by using a dynamic positioning mapping algorithm; The quaternion attitude solution method is used to process the attitude data of the truck crane, and the posture correction of the scene map is performed to obtain the environment correction model including: Each sampling point in the point cloud data is sequentially used as a point to be processed, and the neighboring points of the point to be processed are searched within a preset radius, and the spatial distance weight and the intensity value weight of the point to be processed and the corresponding neighboring points are respectively calculated, and the product of the spatial distance weight and the intensity value weight is used as the comprehensive weight, and the position of the point to be processed is updated based on the comprehensive weight to obtain the filtered point cloud data; Calculate the normal vector and principal curvature value of each point in the filtered point cloud data, select the local extreme point of the principal curvature value as the feature point, construct the local descriptor of the feature point, perform feature matching on the local descriptor through fast neighbor search, remove the wrong matching points in combination with the random sampling consistency algorithm, determine the retained matching points, calculate the pose transformation matrix based on the retained matching points, and obtain the point cloud registration data; Determine the initial pose based on the pose transformation matrix, extract the local plane features in the point cloud registration data to construct a pose graph, calculate the point cloud distance field value between consecutive frames, compare the point cloud distance field value with the adaptive threshold to identify the dynamic point, assign attenuation weights to the local plane features corresponding to the dynamic point, construct a weighted pose optimization equation, solve it to obtain the optimized pose, transform the point cloud of the current frame to the global coordinate system through the optimized pose, and update the scene map; Based on the optimized posture and the point cloud motion information in the scene map, a quaternion equation describing the spatial posture is constructed, the quaternion equation is numerically integrated to obtain a predicted posture, and an error state equation is constructed based on the deviation between the predicted posture and the actual observed posture and solved to obtain a posture quaternion; Calculate each element of the rotation matrix according to the posture quaternion, perform coordinate transformation on the point cloud position and normal vector in the scene map through the rotation matrix, update local plane parameters based on the transformed point cloud position and normal vector, reconstruct the spatial position relationship of the feature points, and generate an environment correction model.

3. The method according to claim 1, characterized in that The environment correction model and the obstacle distance data are spatially fused through iterative closest point registration to construct a dynamic environment map; obstacle features in the dynamic environment map are identified through a target detection algorithm combined with Kalman filtering, and obstacle status data is obtained, including: Taking the feature point set in the environmental correction model as the first feature point set, taking the feature point set in the obstacle distance data as the second feature point set, taking each feature point in the first feature point set as a reference feature point, searching for the nearest neighbor point in the second feature point set, and constructing a point pair set with the reference feature point and the nearest neighbor point; Calculate the centroid coordinates based on the point pair set, calculate the relative position of each point in the point pair set relative to the centroid coordinates, construct a covariance matrix of the relative positions, and perform singular value decomposition on the covariance matrix to obtain a rotation matrix and a translation vector; Based on the rotation matrix and the translation vector, coordinate transformation is performed on the reference feature point, and the nearest neighbor point search, centroid coordinate calculation and coordinate transformation are repeatedly performed until the position change of the reference feature point is less than a preset threshold value, thereby obtaining a dynamic environment map; Clustering and segmenting the point cloud in the dynamic environment map, extracting the position center, main direction vector and size parameter of each cluster, constructing a feature vector, and establishing an obstacle state vector including a position vector, a velocity vector and an acceleration vector based on the feature vector; A state transfer equation is constructed according to the obstacle state vector at adjacent moments, and an observation equation is constructed by taking the characteristic vector as an observation value. Based on the state transfer equation and the observation equation, a deviation between a predicted value and an observed value of the obstacle state vector is calculated, and the obstacle state vector is updated using the deviation to obtain obstacle state data.

4. The method according to claim 1, characterized in that: Based on the dynamic environment map and in combination with the obstacle status data, dividing the initial operation area by using an area boundary generation algorithm includes: Calculate the outer polygon of the point cloud data in the dynamic environment map, select three non-collinear feature points within the scope of the outer polygon to construct a triangular mesh, insert the remaining point cloud data into the triangular mesh in sequence, iteratively adjust the triangular mesh by maximizing the minimum inner angle of the triangle, and if the minimum inner angle of the triangle after adjustment is greater than the minimum inner angle of the triangle before adjustment, update the connection relationship of the triangular mesh until all the point cloud data are added to the triangular mesh; In the triangular mesh, for each triangle, mark it as the current triangle, determine whether there is a circle with a preset radius that passes through the three vertices of the current triangle, and the interior of the circle does not contain any point in the point cloud data. If so, the current triangle is a valid triangle; extract the edge segments of all the valid triangles, determine the edge segment set, take the segment that appears once in the edge segment set as the target boundary segment, and reorganize the target boundary segment into a closed area boundary curve in a way that the endpoints are connected; Based on the position vector and velocity vector in the obstacle state data, the movement trajectory of the obstacle is predicted within a preset time window, and the expansion area containing the current position and movement trajectory of the obstacle is calculated as the obstacle influence area. The area enclosed by the area boundary curve is superimposed with the obstacle influence area to obtain the initial operation area.

5. The method according to claim 4, characterized in that Using the convex hull decomposition algorithm, the final operation area is determined to include: Connect any two convex vertices in the initial operation area, check whether the line between the two convex vertices has an intersection with the area boundary curve, and construct the lines without intersections into a connected graph; Selecting a concave vertex with the smallest inner angle in the initial operation area as a segmentation starting point, selecting an optimal segmentation line in the connected graph to divide the area into a first sub-area and a second sub-area, performing segmentation steps on the first sub-area and the second sub-area respectively until each sub-area after segmentation becomes a convex polygon, obtaining a convex polygon sub-area, and forming all the convex polygon sub-areas into an area set; Calculate the area change of adjacent convex polygonal sub-regions with common edges in the region set after merging. When the merged region remains a convex polygon and the area change is less than a preset change threshold, merge the adjacent convex polygonal sub-regions into a new sub-region. Repeat the merging until there are no sub-regions that can be merged, and obtain the final working area.

6. The method according to claim 1, characterized in that Based on the workspace safety level model and the obstacle state data, a fast random tree connection method combined with dynamic programming is used to generate an obstacle avoidance candidate path that meets the load constraint, including: Establishing a state space of the truck crane, the state space including configuration parameters and motion parameters of the truck crane, and establishing a state transfer cost function based on the state space, the state transfer cost function including a distance cost between state points, a path curvature cost, and a safety level cost based on a work space safety level model; In the state space, determine a starting state point and a target state point, respectively establish bidirectional random trees with the starting state point and the target state point as root nodes, including a forward random tree and a reverse random tree, and randomly sample in the state space to obtain candidate state points; Taking the forward random tree or the reverse random tree as the current random tree, determining the tree node closest to the candidate state point in the current random tree, and extending a preset distance from the tree node toward the candidate state point to obtain an extended node; Based on the workspace safety level model, determine the safety level of the extension node, and based on the obstacle status data, determine whether the path from the tree node to the extension node intersects with an obstacle, and when the safety level of the extension node meets the preset area and does not intersect with the obstacle, add the extension node to the current random tree; Determine whether there is a connection path between the bidirectional random trees, generate an initial obstacle avoidance path when there is a connection path, sample at the path nodes of the initial obstacle avoidance path to obtain a set of candidate nodes, calculate the path cost of each node in the set of candidate nodes based on the state transition cost function; and generate a set of obstacle avoidance candidate paths that meet the load constraint based on the path cost using a dynamic programming method.

7. The method according to claim 1, characterized in that The optimal obstacle avoidance path is selected according to the comprehensive score calculated based on the path safety and load margin, and the optimal obstacle avoidance path is converted into an obstacle avoidance trajectory and control instructions through a quintic polynomial interpolation algorithm, including: For each candidate path in the candidate path set, a discrete point sequence on the candidate path is calculated, and for each discrete point in the discrete point sequence, a distance from the discrete point to the nearest obstacle is calculated, and a ratio of the distance to a preset safety distance threshold is used as the path safety; For each discrete point in the discrete point sequence, a ratio of the rated lifting weight to the actual lifting weight of the discrete point is calculated as a safety factor, and a ratio of the safety factor to a preset reference safety factor is used as a load margin; Performing a weighted combination of the path safety and the load margin to obtain a path comprehensive score, and selecting a candidate path with the highest path comprehensive score as the optimal obstacle avoidance path; Selecting two adjacent path nodes on the optimal obstacle avoidance path, and establishing a quintic polynomial trajectory equation based on position constraints, velocity constraints, and acceleration constraints of the two adjacent path nodes; The quintic polynomial trajectory equation is sampled to obtain the position sequence, velocity sequence and acceleration sequence of the end effector in Cartesian space; the position sequence is transformed through inverse kinematics to obtain the angle sequence in the joint space, and the angle sequence is numerically differentiated to obtain the velocity sequence in the joint space; the angle sequence and the velocity sequence are converted into a motor speed sequence and a cylinder extension and retraction speed sequence to generate corresponding control instructions.

8. Intelligent obstacle avoidance and operation range planning system for manual telescopic arm of truck-mounted crane, used to implement the method described in any one of claims 1 to 7, characterized in that: include: The first unit is used to collect environmental point cloud data around the truck crane, the truck crane posture data and obstacle distance data; After the point cloud data is subjected to denoising and point cloud registration processing by using a bilateral filtering algorithm, a scene map is obtained by using a dynamic positioning mapping algorithm; the truck crane attitude data is processed by using a quaternion attitude solution method, and the scene map is subjected to posture correction to obtain an environmental correction model; the environmental correction model is spatially fused with the obstacle distance data by iterative closest point registration to construct a dynamic environment map; obstacle features in the dynamic environment map are identified by combining a target detection algorithm with a Kalman filter to obtain obstacle status data; The second unit is used to divide the initial operation area by using the area boundary generation algorithm based on the dynamic environment map and the obstacle state data, and to determine the final operation area by using the convex hull decomposition algorithm; Establishing a boom kinematics model based on a joint geometric parameter method, calculating the safe load capacity distribution within the final operating area, grading the final operating area according to the safe load capacity distribution, and establishing an operating space safety grade model; The third unit is used to generate a candidate obstacle avoidance path that meets the load constraint based on the working space safety level model and the obstacle status data by using a fast random tree connection method combined with dynamic programming; select the optimal obstacle avoidance path according to the comprehensive score calculated based on the path safety and load margin, and convert the optimal obstacle avoidance path into an obstacle avoidance trajectory and control instructions through a quintic polynomial interpolation algorithm; use a real-time distance field algorithm to monitor the distance between the boom and the obstacle, and execute prompts, warnings and control instructions based on a preset hierarchical control threshold.

9. An electronic device, characterized in that: include: processor; a memory for storing processor-executable instructions; The processor is configured to call the instructions stored in the memory to execute the method described in any one of claims 1 to 7.

10. A computer-readable storage medium having computer program instructions stored thereon, characterized in that: When the computer program instructions are executed by a processor, the method according to any one of claims 1 to 7 is implemented.

Images