Shovel lane collision detection and control method, device, equipment and medium

Abstract

The present application belongs to the field of automatic control technology of engineering machinery, and provides a kind of shovel truck roadway collision detection and control method, device, equipment and medium, wherein the method comprises: according to the three-dimensional grid model of roadway and the forward kinematics model of working component, dynamic scanning data, real-time attitude information and joint angle data, the six-degree-of-freedom pose of shovel truck in roadway coordinate system is determined, and the pose of shovel truck relative to vehicle body coordinate system and the accurate pose of shovel bucket under the global coordinate system of roadway are determined;Collision detection algorithm is used to detect the collision of three-dimensional grid model of roadway and shovel bucket, collision prediction is carried out according to preset safety distance threshold and joint angle data, and collision prediction result is obtained;According to the collision prediction result, warning signal and control signal are issued to the control system of shovel truck, and the reliability of collision detection is improved and the consumption of system resources is reduced by the above technical scheme.

Description

Technical Field

[0001] This invention relates to the field of automatic control technology for construction machinery, and in particular to a method, device, equipment and medium for detecting and controlling collisions in roadways for loader trucks. Background Technology

[0002] The intelligent development of underground metal mines is rapid. Automation technology for fixed positions is relatively mature, while the intelligentization of mobile equipment is becoming a research hotspot. As a key piece of equipment in mine loading operations, the intelligent transformation of loaders is the foundation of mine intelligence. Currently, mines are already piloting remote-controlled operation of loaders. However, research on the intelligentization of loader operations in specific scenarios is scarce, such as the unmanned operation of loaders loading ore into ore trucks, where achieving unmanned operation remains a challenge.

[0003] Numerous safety issues arise during ore loading operations, such as improper coordination between the loader and the ore truck, ore falling from a height and directly impacting the truck or driver during loading, collisions between the loader and the ore truck or surrounding roadways, and operators failing to comply with safety procedures. Furthermore, the ore loading environment is typically complex, accompanied by high noise and dust pollution, all of which adversely affect the health and safety of operators. Therefore, achieving unmanned ore loading operations is crucial for reducing workplace accidents and improving operational safety.

[0004] Because loaders and ore trucks operate in narrow and complex tunnels, in addition to the risks of human error, there is also the risk of collision. Therefore, to successfully achieve unmanned ore loading operations, potential collisions must be fully considered. During the loading process, the loader arm needs to be raised beforehand. As the arm is raised, due to the limited tunnel space, the bucket is likely to collide with the tunnel, causing safety issues. Existing technologies attempt to use sensors such as lidar to scan the environment in real time and detect collisions by processing point cloud data. This type of method belongs to the real-time perception-response mode, and its effectiveness is highly dependent on the sensor performance, installation location, and operating environment. In the harsh environment of underground mines with high dust, water mist, and poor lighting conditions, the quality of point cloud data may deteriorate sharply, resulting in significant noise or missing data. Furthermore, the loader arm may obstruct the lidar's scanning field of view during movement, causing critical areas (such as the bucket tip) to be unobservable, thus creating blind spots and posing safety hazards. Meanwhile, processing massive amounts of real-time point cloud data places extremely high demands on the computing power of the onboard computing unit, which may introduce latency and affect the real-time performance of collision detection. Summary of the Invention

[0005] In order to at least solve one of the technical problems existing in the prior art, the present invention provides a method, device, equipment and medium for detecting and controlling collisions in roadways for loader trucks.

[0006] One aspect of the present invention provides a method for collision detection and control of a loader in a roadway, comprising:

[0007] Acquire static scan data of the tunnel environment and generate a 3D mesh model of the tunnel based on the static scan data;

[0008] The rigid body linkage model of the loader is determined based on the three-dimensional design data of the loader, and the forward kinematic model of the working parts of the loader is determined based on the rigid body linkage model.

[0009] The dynamic scanning data of the tunnel environment and the real-time attitude information of the loader are obtained. Based on the dynamic scanning data, real-time attitude information and the three-dimensional mesh model of the tunnel, the six-degree-of-freedom pose of the loader in the tunnel coordinate system is determined.

[0010] The joint rotation data collected by the angle sensors set at the joints of the loader are obtained. Based on the joint rotation data, the positive kinematic model of the working parts and the six-degree-of-freedom pose, the pose of the loader's bucket relative to the vehicle coordinate system and the precise pose of the bucket in the roadway global coordinate system are determined.

[0011] Based on the pose of the bucket relative to the vehicle body coordinate system and the precise pose of the bucket in the global coordinate system of the roadway, a collision detection algorithm is used to perform collision detection on the three-dimensional mesh model of the roadway and the bucket. Based on the detection results, the preset safe distance threshold and the joint angle data, collision prediction is performed to obtain the collision prediction result.

[0012] Based on the collision prediction results, at least one of the warning signal and control signal is sent to the control system of the loader.

[0013] According to the aforementioned method for collision detection and control of loaders in roadways, acquiring scan data of the roadway environment and generating a three-dimensional mesh model of the roadway based on the scan data includes:

[0014] Acquire high-precision scanning data obtained after a laser scanning vehicle or drone scans the tunnel;

[0015] High-precision scanning data is used to generate point cloud data, and a three-dimensional mesh model of the tunnel is generated based on the point cloud data.

[0016] According to the aforementioned method for detecting and controlling roadway collisions with a loader, the rigid body linkage model of the loader is determined based on its three-dimensional design data, and the kinematic model of the loader's working components is determined based on the rigid body linkage model, including:

[0017] Based on the joint type of the working parts in the rigid body linkage model, the DH parameter method is used to generate a positive kinematic model from the vehicle body base coordinate system to the bucket end coordinate system. The working parts include at least the vehicle body, boom, and bucket.

[0018] Based on the forward kinematics model, calculate the homogeneous transformation matrix between adjacent joints of the loader's working parts;

[0019] Based on the homogeneous transformation matrix between adjacent joints of the working parts, calculate the homogeneous transformation matrix from the hinge point of the boom and base to the tip of the bucket teeth, and then determine the coordinate equation of the tip of the bucket teeth based on the homogeneous transformation matrix from the hinge point to the tip of the bucket teeth.

[0020] According to the aforementioned method for detecting and controlling roadway collisions of a loader, dynamic scanning data of the roadway environment and real-time attitude information of the loader are acquired. Based on the dynamic scanning data, real-time attitude information, and a three-dimensional mesh model of the roadway, the six-degree-of-freedom pose of the loader in the roadway coordinate system is determined, including:

[0021] The system acquires dynamic scanning data obtained from the LiDAR installed on the loader, and real-time attitude information collected by the inertial measurement unit installed on the loader.

[0022] The dynamic scanning data is matched with the three-dimensional mesh model of the tunnel. Based on the matching results and real-time attitude information, the extended Kalman filter fusion algorithm is used to determine the six-degree-of-freedom pose of the loader in the tunnel coordinate system.

[0023] According to the aforementioned method for detecting and controlling roadway collisions of a loader, the method acquires joint angle data collected by angle sensors installed at the joints of the loader. Based on the joint angle data, the forward kinematic model of the working parts, and the six-degree-of-freedom pose, the method determines the pose of the loader's bucket relative to the vehicle body coordinate system and the precise pose of the bucket in the roadway global coordinate system, including:

[0024] Acquire joint rotation data from angle sensors installed on at least two booms, wherein the joint rotation data is the absolute rotation angle of the joint;

[0025] Based on the joint rotation angle data, the positive kinematic model of the working parts is used for calculation to obtain the transformation matrix of the bucket relative to the vehicle body;

[0026] The precise pose of the bucket in the global coordinate system of the roadway map is determined by matrix multiplication based on the six-degree-of-freedom pose and the transformation matrix of the bucket relative to the vehicle body.

[0027] According to the aforementioned method for collision detection and control of a loader in a roadway, a collision detection algorithm is used to perform collision detection on the three-dimensional mesh model of the roadway and the bucket based on the pose of the bucket relative to the vehicle body coordinate system and the precise pose of the bucket in the global coordinate system of the roadway. Based on the detection results, a preset safety distance threshold, and joint angle data, collision prediction is performed to obtain the collision prediction results, including:

[0028] The precise pose of the bucket in the global coordinate system of the tunnel map and the 3D mesh model of the tunnel are coarsely detected using a collision detection library. The coarse detection includes whether the directed bounding box of the bucket intersects with the bounding box of the octree node of the tunnel model.

[0029] If the detections intersect, the GJK algorithm is used for fine detection, which includes using the GJK algorithm to calculate the minimum distance between the bucket and the roadway grid within the octagonal leaf node.

[0030] The detection results are obtained based on the minimum distance and the preset safe distance threshold;

[0031] Differential processing is performed on the joint rotation angle data to obtain real-time angular velocity and real-time angular acceleration;

[0032] Using a uniform acceleration motion model, we obtain joint angle data, real-time angular velocity, and real-time angular acceleration, and predict joint angle sequences for future moments.

[0033] Based on the predicted joint angle sequence, perform predicted pose calculation, coarse detection, and fine detection to obtain collision prediction results.

[0034] According to the aforementioned method for detecting and controlling roadway collisions with a loader, the method further includes sending at least one of a warning signal and a control signal to the loader's control system based on the collision prediction result, and also includes:

[0035] If the minimum distance is less than the preset safe distance threshold but greater than the emergency braking threshold, a warning signal is sent to the control system of the loader.

[0036] If the minimum distance is less than the preset safe distance threshold and less than or equal to the emergency braking threshold, a warning signal and a control signal are sent to the control system of the loader.

[0037] Another aspect of the present invention provides a collision detection and control device for a loader in a roadway, comprising:

[0038] The first module is used to acquire static scanning data of the tunnel environment and generate a three-dimensional mesh model of the tunnel based on the static scanning data.

[0039] The second module is used to determine the rigid body linkage model of the loader based on the three-dimensional design data of the loader, and to determine the forward kinematic model of the working parts of the loader based on the rigid body linkage model.

[0040] The third module is used to acquire dynamic scanning data of the tunnel environment and real-time attitude information of the loader. Based on the dynamic scanning data, real-time attitude information and the three-dimensional mesh model of the tunnel, the six-degree-of-freedom pose of the loader in the tunnel coordinate system is determined.

[0041] The fourth module is used to acquire the joint rotation data collected by the angle sensors installed at the joints of the loader. Based on the joint rotation data, the positive kinematic model of the working parts and the six-degree-of-freedom pose, the pose of the loader's bucket relative to the vehicle coordinate system and the precise pose of the bucket in the roadway global coordinate system are determined.

[0042] The fifth module is used to perform collision detection on the 3D mesh model of the roadway and the bucket based on the pose of the bucket relative to the vehicle coordinate system and the precise pose of the bucket in the global coordinate system of the roadway. Based on the detection results, preset safety distance thresholds and joint angle data, collision prediction is performed to obtain the collision prediction results.

[0043] The sixth module is used to send at least one of the warning signal and control signal to the control system of the loader based on the collision prediction results.

[0044] Another aspect of the present invention provides an electronic device, including a processor and a memory;

[0045] The memory is used to store programs;

[0046] The processor executes the program to implement the method as described above.

[0047] This invention also discloses a computer program product or computer program, which includes computer instructions stored in a computer-readable storage medium. A processor of a computer device can read the computer instructions from the computer-readable storage medium and execute the computer instructions, causing the computer device to perform the methods described above.

[0048] The beneficial effects of this invention are as follows: By using a pre-built model and its own state calculation, a digital twin model of the tunnel and equipment is achieved, rather than real-time environmental scanning. The reliability of collision detection then primarily depends on positioning accuracy and joint angle measurement accuracy, which is more reliable than relying on unstable point cloud data. Through real-time calculation of the bucket pose using a forward kinematics model, the bucket's position in digital space is precisely known regardless of its orientation, fundamentally eliminating detection blind spots caused by physical occlusion. By predicting impending collisions in the digital model, a longer reaction time is provided for the control system, enabling smoother deceleration or avoidance, thus improving operational safety and smoothness. Compared to performing a series of complex processes such as filtering, downsampling, clustering, and segmenting on millions of raw point clouds per frame, this invention transforms the problem into an intersection operation between two deterministic geometric models (bucket model and tunnel model), reducing system resource consumption, lowering the computational requirements of onboard hardware, and minimizing system latency. Attached Figure Description

[0049] Figure 1 This is a schematic diagram of the collision detection and control process for a loader in a roadway according to an embodiment of the present invention;

[0050] Figure 2 This is a DH model diagram of the loader working device according to an embodiment of the present invention;

[0051] Figure 3 This is a simplified model schematic diagram of the working device of the loader according to an embodiment of the present invention;

[0052] Figure 4 This is a schematic diagram of a roadway collision detection and control device for a loader according to an embodiment of the present invention. Detailed Implementation

[0053] The embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings. Throughout the description, the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions. In the following description, suffixes such as "module," "part," or "unit" used to denote elements are used only for the purpose of illustrative purposes and have no specific meaning in themselves. Therefore, "module," "part," or "unit" can be used interchangeably. Terms such as "first," "second," etc., are used only to distinguish technical features and should not be construed as indicating or implying relative importance, or implicitly indicating the number of indicated technical features, or implicitly indicating the sequential relationship of the indicated technical features. In the following description, the consecutive reference numerals for method steps are for ease of review and understanding. Adjusting the implementation order of steps, in conjunction with the overall technical solution of the present invention and the logical relationship between the various steps, will not affect the technical effect achieved by the technical solution of the present invention. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and should not be construed as limiting the present invention.

[0054] refer to Figure 1 , Figure 1 This is a schematic diagram of the collision detection and control process for a loader in a roadway according to an embodiment of the present invention, which includes, but is not limited to, steps S100 to S600:

[0055] S100, static scan data of the environment, and a three-dimensional mesh model of the tunnel is generated based on the static scan data.

[0056] In some embodiments, high-precision scanning data obtained after a laser scanning vehicle or drone scans the tunnel is acquired; point cloud data is generated from the high-precision scanning data; and a three-dimensional mesh model of the tunnel is generated based on the point cloud data.

[0057] For example, during non-operational periods in the tunnel, a high-precision 3D laser scanner (such as a Leica RTC360) is used to comprehensively scan the target work area to acquire raw point cloud data. Multi-station data is then stitched together using point cloud registration software (such as CloudCompare), followed by noise reduction and simplification. A closed, non-self-intersecting triangular mesh model (such as .STL or .OBJ format) is then generated using surface reconstruction algorithms (such as Poisson reconstruction). This model is loaded into the vehicle-mounted computing unit as an immutable, static environment map.

[0058] The three-dimensional design data of S200 determines the rigid body linkage model of the loader, and the forward kinematic model of the working parts of the loader is determined based on the rigid body linkage model.

[0059] In some embodiments, based on the joint type of the working parts in the rigid body linkage model, a forward kinematic model from the vehicle body base coordinate system to the bucket end coordinate system is obtained using the DH parameter method, wherein the working parts include at least the vehicle body, boom, and bucket; based on the forward kinematic model, the homogeneous transformation matrix between adjacent joints of the working parts of the loader is calculated; based on the homogeneous transformation matrix between adjacent joints of the working parts, the homogeneous transformation matrix from the hinge point of the boom and base to the bucket tooth tip is calculated, and then the coordinate equation of the bucket tooth tip is determined based on the homogeneous transformation matrix from the hinge point to the bucket tooth tip.

[0060] For example, this embodiment of the invention obtains a precise 3D CAD model of a loader. In modeling software (such as SolidWorks or Blender), it is simplified into main rigid body components: body, boom, rocker arm, linkage, and bucket. The joint types between each component are defined; for example, the connection between the boom and body is a rotary joint, and the connection between the bucket and rocker arm is also a rotary joint. A forward kinematic model from the body base coordinate system to the bucket end-effector coordinate system is established using the standard Denavit-Hartenberg (DH) parametric method, thereby obtaining the bucket end-effector coordinate equation. This equation takes the angles of each joint as input and outputs the pose matrix of the bucket relative to the body.

[0061] refer to Figure 2 The DH model of the loader working device shown is shown. This is the hinge point between the boom and the base. This is the hinge point between the bucket and the boom. Located at the tip of the bucket teeth. for , The length between, for , The length between, for and The angle between them for and The angle between them.

[0062] Furthermore, based on the established DH parameter model, the homogeneous transformation matrices between adjacent joints of the loader's working device are calculated as follows:

[0063] ;

[0064] ;

[0065] The coordinates of the bucket tooth tip can be obtained by simultaneously solving the above equations. and The homogeneous transformation matrix between them is:

[0066] ;

[0067] Therefore, the coordinates of the bucket tooth tip are: .

[0068] Based on the dynamic scanning data of the environment and the real-time attitude information of the loader, and the three-dimensional mesh model of the tunnel, the six-degree-of-freedom pose of the loader in the tunnel coordinate system is determined.

[0069] In some embodiments, dynamic scanning data obtained by the lidar installed on the loader is acquired, and real-time attitude information collected by the inertial measurement unit installed on the loader is acquired; the dynamic scanning data is matched with the three-dimensional mesh model of the roadway, and the six-degree-of-freedom pose of the loader in the roadway coordinate system is determined based on the matching result and the real-time attitude information and by using the extended Kalman filter fusion algorithm.

[0070] For example, a 32-line LiDAR and an industrial-grade IMU are mounted on the vehicle body for positioning. When the system is running, the LiDAR scans the surrounding lanes in real time, matches the scan results with the high-precision grid map in the S100 example (Scan-to-Model matching), and combines the attitude information provided by the IMU with the extended Kalman filter (EKF) fusion algorithm to output the high-frequency (e.g., 50Hz) pose estimate of the vehicle body in the map coordinate system in real time, that is, to obtain the six-degree-of-freedom pose (position and attitude) of the loader in the lane coordinate system.

[0071] S400 uses angle sensors at the joints of the loader to collect joint angle data. Based on the joint angle data, the positive kinematic model of the working parts, and the six-degree-of-freedom pose, the pose of the loader's bucket relative to the vehicle coordinate system and the precise pose of the bucket in the roadway global coordinate system are determined.

[0072] In some embodiments, joint rotation data from angle sensors mounted on at least two booms (such as rocker arms and booms) are acquired, wherein the joint rotation data is the absolute rotation angle of the joint; based on the joint rotation data, a forward kinematic model of the working parts is used to calculate the transformation matrix of the bucket relative to the vehicle body; based on the six-degree-of-freedom pose and the transformation matrix of the bucket relative to the vehicle body, matrix multiplication is used to determine the precise pose of the bucket in the global coordinate system of the roadway map.

[0073] In some embodiments, the angle sensors are absolute angle sensors, such as tilt sensors. Specifically, high-resolution absolute angle sensors are installed at the root pivot of the boom and the root pivot of the rocker arm, respectively. These sensors directly measure the absolute rotation angle of the joint and transmit it to the vehicle controller in real time (e.g., 100Hz) via a bus.

[0074] For example, refer to Figure 3 The simplified model diagram of the loader's working device shown illustrates the installation positions of three tilt sensors. The tilt sensor measures the angle between its body and the horizontal direction; upward horizontal is positive, and downward horizontal is negative. Tilt sensor 1 is located along the boom hinge point. and joystick hinge point The direction of the wiring is installed, and the measured value is... Tilt sensor 2 along Direction, measured value Tilt sensor 3 along Direction, measured value .

[0075] Furthermore, based on the above Figure 3 The process of calculating the precise pose in the global coordinate system of the tunnel map is as follows:

[0076] First, determine the boom rotation angle. Perform the solution. for and The angle between them.

[0077] ;

[0078] In triangle In the middle, the lengths of the three sides , , These are all fixed parameters for the loader, which can be obtained using the law of cosines. Size.

[0079] ;

[0080] Next, solve for the angle. ,

[0081] ;

[0082] 、 、 These are all fixed parameters for the loader.

[0083] For the S500, the pose of the vehicle body coordinate system and the precise pose of the bucket in the global coordinate system of the roadway are obtained by using a collision detection algorithm to perform collision detection on the three-dimensional mesh model of the roadway and the bucket. Based on the detection results, the preset safe distance threshold and the joint angle data, the collision prediction results are obtained.

[0084] In some embodiments, the precise pose of the bucket in the global coordinate system of the roadway map and the 3D mesh model of the roadway are coarsely detected using a collision detection library. The coarse detection includes checking whether the directed bounding box of the bucket intersects with the bounding box of the octree node of the roadway model. If an intersection is detected, the GJK algorithm is used for fine detection. The fine detection includes using the GJK algorithm to calculate the minimum distance between the bucket and the roadway mesh patch within the leaf node of the octree. The detection result is obtained based on the minimum distance and a preset safe distance threshold. The joint angle data is differentially processed to obtain the real-time angular velocity and real-time angular acceleration. A uniform acceleration motion model is used to obtain the predicted joint angle sequence of the joint angle data, real-time angular velocity, and real-time angular acceleration at future times. Based on the predicted joint angle sequence, the predicted pose calculation, coarse detection, and fine detection are performed to obtain the collision prediction result.

[0085] For example, the real-time pose of the obtained bucket model is applied to the 3D mesh model of the bucket, and an efficient collision detection library (such as Flexible Collision Library (FCL) or Bullet Physics) is called.

[0086] First, a coarse check is performed to calculate whether the directed bounding box (OBB) of the bucket model intersects with the bounding box of the octree nodes of the tunnel model. If they do not intersect, it is determined that there is no risk of collision.

[0087] If the coarse detection intersects, the fine detection proceeds, including using the GJK (Gilbert-Johnson-Keerthi) algorithm to calculate the minimum distance between the bucket model and the corresponding octagonal leaf node's lane mesh. The GJK algorithm can efficiently calculate the distance between two convex bodies. For non-convex buckets, they can be decomposed into combinations of multiple convex bodies for calculation.

[0088] In yet another embodiment, to achieve forward-looking prediction, based on the current collision detection described above, the following operations are further performed:

[0089] Kinematic parameters are acquired by differential processing of mid-to-high frequency angle sensor data to calculate the real-time angular velocity and angular acceleration of each joint. Trajectory extrapolation is performed using a uniform acceleration motion model to calculate predicted joint angles at future time points such as 0.1 seconds, 0.2 seconds, and up to 0.5 seconds based on the current joint angles, angular velocities, and angular accelerations. Predictive pose calculation involves sequentially performing the above calculation process on these predicted joint angle sequences to calculate the predicted pose of the bucket in the future. Predictive collision detection involves repeatedly performing coarse and fine detection processes for each predicted pose to determine whether there are collision risk points with the tunnel model on the future trajectory.

[0090] S600 sends at least one of the following signals to the control system of the loader: a warning signal and a control signal.

[0091] In some embodiments, if the minimum distance is less than a preset safe distance threshold but greater than an emergency braking threshold, a warning signal is sent to the control system of the loader; if the minimum distance is less than a preset safe distance threshold and less than or equal to an emergency braking threshold, a warning signal and a control signal are sent to the control system of the loader.

[0092] In some embodiments, the preset safe distance threshold can be set to 0.3 meters, and the emergency braking threshold can be set to 0.1 meters.

[0093] Figure 4 This is a schematic diagram of a roadway collision detection and control device for a loader according to an embodiment of the present invention. The device includes a first module 410, a second module 420, a third module 430, a fourth module 440, a fifth module 450, and a sixth module 460.

[0094] The system comprises four modules: The first module acquires static scanning data of the tunnel environment and generates a 3D mesh model of the tunnel based on this data; the second module determines the rigid body linkage model of the loader based on its 3D design data and then determines the forward kinematic model of the loader's working parts based on this model; the third module acquires dynamic scanning data of the tunnel environment and the real-time attitude information of the loader, determining the loader's six-degree-of-freedom pose in the tunnel coordinate system based on the dynamic scanning data, real-time attitude information, and the 3D mesh model of the tunnel; and the fourth module acquires joint angle data collected by angle sensors installed at the joints of the loader. The first module determines the pose of the loader's bucket relative to the vehicle body coordinate system and the precise pose of the bucket in the roadway global coordinate system based on the joint rotation angle data, the positive kinematic model of the working parts, and the six-degree-of-freedom pose. The second module uses a collision detection algorithm to perform collision detection on the roadway 3D mesh model and the bucket based on the pose of the bucket relative to the vehicle body coordinate system and the precise pose of the bucket in the roadway global coordinate system. Based on the detection results, a preset safety distance threshold, and joint rotation angle data, a collision prediction is performed to obtain the collision prediction result. The third module sends at least one of the following to the loader's control system based on the collision prediction result: a warning signal and a control signal.

[0095] Exemplarily, with the cooperation of the first, second, third, fourth, fifth, and sixth modules in the device, the embodiment device can implement any of the aforementioned methods for detecting and controlling roadway collisions of a loader, namely, acquiring static scanning data of the roadway environment and generating a three-dimensional mesh model of the roadway based on the static scanning data; determining the rigid body linkage model of the loader based on the three-dimensional design data of the loader, and determining the forward kinematic model of the working parts of the loader based on the rigid body linkage model; acquiring dynamic scanning data of the roadway environment and real-time attitude information of the loader, and determining the six-degree-of-freedom pose of the loader in the roadway coordinate system based on the dynamic scanning data, real-time attitude information, and the three-dimensional mesh model of the roadway. The system acquires joint angle data from angle sensors located at the joints of the loader. Based on the joint angle data, the forward kinematic model of the working parts, and the six-degree-of-freedom pose, it determines the pose of the loader's bucket relative to the vehicle body coordinate system and the precise pose of the bucket in the roadway global coordinate system. Based on the pose of the bucket relative to the vehicle body coordinate system and the precise pose of the bucket in the roadway global coordinate system, it uses a collision detection algorithm to perform collision detection on the roadway 3D mesh model and the bucket. Based on the detection results, a preset safety distance threshold, and joint angle data, it performs collision prediction to obtain the collision prediction result. Based on the collision prediction result, it sends at least one of a warning signal and a control signal to the loader's control system. The beneficial effects of this invention are as follows: By using a pre-built model and its own state calculation, a digital twin model of the tunnel and equipment is achieved, rather than real-time environmental scanning. The reliability of collision detection then primarily depends on positioning accuracy and joint angle measurement accuracy, which is more reliable than relying on unstable point cloud data. Through real-time calculation of the bucket pose using a forward kinematics model, the bucket's position in digital space is precisely known regardless of its orientation, fundamentally eliminating detection blind spots caused by physical occlusion. By predicting impending collisions in the digital model, a longer reaction time is provided for the control system, enabling smoother deceleration or avoidance, thus improving operational safety and smoothness. Compared to performing a series of complex processes such as filtering, downsampling, clustering, and segmenting on millions of raw point clouds per frame, this invention transforms the problem into an intersection operation between two deterministic geometric models (bucket model and tunnel model), reducing system resource consumption, lowering the computational requirements of onboard hardware, and minimizing system latency.

[0096] This invention also provides an electronic device, which includes a processor and a memory;

[0097] The memory stores the program;

[0098] The processor executes a program to perform the aforementioned method for detecting and controlling collisions in roadways for loader machines; the electronic device has the function of carrying and running the software system for detecting and controlling collisions in roadways for loader machines provided in the embodiments of the present invention, such as a personal computer, minicomputer, mainframe, workstation, network or distributed computing environment, standalone or integrated computer platform, or communicating with charged particle tools or other imaging devices, etc.

[0099] This invention also provides a computer-readable storage medium storing a program that is executed by a processor to implement the loader roadway collision detection and control method described above.

[0100] In some alternative embodiments, the functions / operations mentioned in the block diagrams may not occur in the order shown in the operation diagrams. For example, depending on the functions / operations involved, two consecutively shown blocks may actually be executed substantially simultaneously, or the blocks may sometimes be executed in reverse order. Furthermore, the embodiments presented and described in the flowcharts of this invention are provided by way of example to provide a more comprehensive understanding of the technology. The disclosed methods are not limited to the operations and logic flows presented in the embodiments of this invention. Alternative embodiments are contemplated, in which the order of various operations is changed and sub-operations described as part of a larger operation are executed independently.

[0101] This invention also discloses a computer program product or computer program, which includes computer instructions stored in a computer-readable storage medium. A processor of a computer device can read the computer instructions from the computer-readable storage medium and execute the computer instructions, causing the computer device to perform the aforementioned method for detecting and controlling collisions in roadways for loader operators.

[0102] Furthermore, although the invention has been described in the context of functional modules, it should be understood that, unless otherwise stated, one or more of the described functions and / or features may be integrated into a single physical device and / or software module, or one or more functions and / or features may be implemented in a separate physical device or software module. It is also understood that a detailed discussion of the actual implementation of each module is unnecessary for understanding the invention. Rather, considering the properties, functions, and internal relationships of the various functional modules in the apparatus disclosed in the embodiments of the invention, the actual implementation of the module will be understood within the scope of conventional skill of an engineer. Therefore, those skilled in the art can implement the invention as set forth in the claims using ordinary techniques without excessive experimentation. It is also understood that the specific concepts disclosed are merely illustrative and are not intended to limit the scope of the invention, which is determined by the full scope of the appended claims and their equivalents.

[0103] If the aforementioned functions are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this invention, essentially, or the part that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0104] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a sequenced list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (such as a computer-based system, a processor-included system, or other system that can fetch and execute instructions from, an instruction execution system, apparatus, or device). For the purposes of this specification, "computer-readable medium" can be any means that can include, store, communicate, propagate, or transmit programs for use by, or in conjunction with, an instruction execution system, apparatus, or device.

[0105] More specific examples of computer-readable media (a non-exhaustive list) include: electrical connections (electronic devices) having one or more wires, portable computer disk drives (magnetic devices), random access memory (RAM), read-only memory (ROM), erasable and editable read-only memory (EPROM or flash memory), fiber optic devices, and portable optical disc read-only memory (CDROM). Furthermore, computer-readable media can even be paper or other suitable media on which the program can be printed, because the program can be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, interpreting, or otherwise processing as necessary, and then stored in computer memory.

[0106] It should be understood that various parts of the present invention can be implemented in hardware, software, firmware, or a combination thereof. In the above embodiments, multiple steps or methods can be implemented in software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, it can be implemented using any one or a combination of the following techniques known in the art: discrete logic circuits having logic gates for implementing logical functions on data signals, application-specific integrated circuits (ASICs) having suitable combinational logic gates, programmable gate arrays (PGAs), field-programmable gate arrays (FPGAs), etc.

[0107] In the description of this specification, references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0108] Although embodiments of the invention have been shown and described, those skilled in the art will understand that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the claims and their equivalents.

[0109] The above is a detailed description of the preferred embodiments of the present invention, but the present invention is not limited to the embodiments described. Those skilled in the art can make various equivalent modifications or substitutions without departing from the spirit of the present invention, and these equivalent modifications or substitutions are all included within the scope defined by the claims of this application.

Claims

1. A method for collision detection and control of a loader in a roadway, characterized in that, include: Acquire static scan data of the tunnel environment and generate a 3D mesh model of the tunnel based on the static scan data; The rigid body linkage model of the loader is determined based on the three-dimensional design data of the loader, and the forward kinematic model of the working parts of the loader is determined based on the rigid body linkage model. The dynamic scanning data of the tunnel environment and the real-time attitude information of the loader are obtained. Based on the dynamic scanning data, real-time attitude information and the three-dimensional mesh model of the tunnel, the six-degree-of-freedom pose of the loader in the tunnel coordinate system is determined. The joint rotation data collected by the angle sensors set at the joints of the loader are obtained. Based on the joint rotation data, the positive kinematic model of the working parts and the six-degree-of-freedom pose, the pose of the loader's bucket relative to the vehicle coordinate system and the precise pose of the bucket in the roadway global coordinate system are determined. Based on the pose of the bucket relative to the vehicle body coordinate system and the precise pose of the bucket in the global coordinate system of the roadway, a collision detection algorithm is used to perform collision detection on the three-dimensional mesh model of the roadway and the bucket. Based on the detection results, the preset safe distance threshold and the joint angle data, collision prediction is performed to obtain the collision prediction result. Based on the collision prediction results, at least one of the following should be sent to the control system of the loader: a warning signal and a control signal. The process of determining the rigid body linkage model of the loader based on its three-dimensional design data, and then determining the kinematic model of the loader's working components based on the rigid body linkage model, includes: Based on the joint type of the working parts in the rigid body linkage model, the DH parameter method is used to generate a positive kinematic model from the vehicle body base coordinate system to the bucket end coordinate system. The working parts include at least the vehicle body, boom, and bucket. Based on the forward kinematics model, calculate the homogeneous transformation matrix between adjacent joints of the loader's working parts; Based on the homogeneous transformation matrix between adjacent joints of the working parts, calculate the homogeneous transformation matrix from the hinge point of the boom and base to the tip of the bucket teeth, and then determine the coordinate equation of the tip of the bucket teeth based on the homogeneous transformation matrix from the hinge point to the tip of the bucket teeth. The method involves using a collision detection algorithm to perform collision detection on the 3D mesh model of the roadway and the bucket based on the bucket's pose relative to the vehicle body coordinate system and the bucket's precise pose in the roadway's global coordinate system. Based on the detection results, a preset safety distance threshold, and joint angle data, collision prediction is performed to obtain the collision prediction results, including: The precise pose of the bucket in the global coordinate system of the tunnel map and the 3D mesh model of the tunnel are coarsely detected using a collision detection library. The coarse detection includes whether the directed bounding box of the bucket intersects with the bounding box of the octree node of the tunnel model. If the detections intersect, the GJK algorithm is used for fine detection, which includes using the GJK algorithm to calculate the minimum distance between the bucket and the roadway grid within the octagonal leaf node. The detection results are obtained based on the minimum distance and the preset safe distance threshold; Differential processing is performed on the joint rotation angle data to obtain real-time angular velocity and real-time angular acceleration; Using a uniform acceleration motion model, we obtain joint angle data, real-time angular velocity, and real-time angular acceleration, and predict joint angle sequences for future moments. Based on the predicted joint angle sequence, perform predicted pose calculation, coarse detection, and fine detection to obtain collision prediction results.

2. The method for detecting and controlling collisions in roadways using a loader according to claim 1, characterized in that, The process of acquiring scan data of the tunnel environment and generating a 3D mesh model of the tunnel based on the scan data includes: Acquire high-precision scanning data obtained after a laser scanning vehicle or drone scans the tunnel; High-precision scanning data is used to generate point cloud data, and a three-dimensional mesh model of the tunnel is generated based on the point cloud data.

3. The method for detecting and controlling collisions in roadways using a loader according to claim 1, characterized in that, The process of acquiring dynamic scanning data of the tunnel environment and real-time attitude information of the loader, and determining the six-degree-of-freedom pose of the loader in the tunnel coordinate system based on the dynamic scanning data, real-time attitude information, and a three-dimensional mesh model of the tunnel, includes: The system acquires dynamic scanning data obtained from the LiDAR installed on the loader, and real-time attitude information collected by the inertial measurement unit installed on the loader. The dynamic scanning data is matched with the three-dimensional mesh model of the tunnel. Based on the matching results and real-time attitude information, the extended Kalman filter fusion algorithm is used to determine the six-degree-of-freedom pose of the loader in the tunnel coordinate system.

4. The method for collision detection and control of a loader in a roadway according to claim 3, characterized in that, The process of acquiring joint angle data collected by angle sensors installed at the joints of the loader, and determining the pose of the loader's bucket relative to the vehicle coordinate system and the precise pose of the bucket in the roadway global coordinate system based on the joint angle data, the positive kinematic model of the working parts, and the six-degree-of-freedom pose, includes: Acquire joint rotation data from angle sensors installed on at least two booms, wherein the joint rotation data is the absolute rotation angle of the joint; Based on the joint rotation angle data, the positive kinematic model of the working parts is used for calculation to obtain the transformation matrix of the bucket relative to the vehicle body; The precise pose of the bucket in the global coordinate system of the roadway map is determined by matrix multiplication based on the six-degree-of-freedom pose and the transformation matrix of the bucket relative to the vehicle body.

5. The method for collision detection and control of a loader in a roadway according to claim 1, characterized in that, The step of sending at least one of a warning signal and a control signal to the control system of the loader based on the collision prediction result further includes: If the minimum distance is less than the preset safe distance threshold but greater than the emergency braking threshold, a warning signal is sent to the control system of the loader. If the minimum distance is less than the preset safe distance threshold and less than or equal to the emergency braking threshold, a warning signal and a control signal are sent to the control system of the loader.

6. A collision detection and control device for a loader in a roadway, characterized in that, include: The first module is used to acquire static scanning data of the tunnel environment and generate a three-dimensional mesh model of the tunnel based on the static scanning data. The second module is used to determine the rigid body linkage model of the loader based on the three-dimensional design data of the loader, and to determine the forward kinematic model of the working parts of the loader based on the rigid body linkage model. The third module is used to acquire dynamic scanning data of the tunnel environment and real-time attitude information of the loader. Based on the dynamic scanning data, real-time attitude information and the three-dimensional mesh model of the tunnel, the six-degree-of-freedom pose of the loader in the tunnel coordinate system is determined. The fourth module is used to acquire the joint rotation data collected by the angle sensors installed at the joints of the loader. Based on the joint rotation data, the positive kinematic model of the working parts and the six-degree-of-freedom pose, the pose of the loader's bucket relative to the vehicle coordinate system and the precise pose of the bucket in the roadway global coordinate system are determined. The fifth module is used to perform collision detection on the 3D mesh model of the roadway and the bucket based on the pose of the bucket relative to the vehicle coordinate system and the precise pose of the bucket in the global coordinate system of the roadway. Based on the detection results, preset safety distance thresholds and joint angle data, collision prediction is performed to obtain the collision prediction results. The sixth module is used to send at least one of the early warning signal and control signal to the control system of the loader based on the collision prediction results; The process of determining the rigid body linkage model of the loader based on its three-dimensional design data, and then determining the kinematic model of the loader's working components based on the rigid body linkage model, includes: Based on the joint type of the working parts in the rigid body linkage model, the DH parameter method is used to generate a positive kinematic model from the vehicle body base coordinate system to the bucket end coordinate system. The working parts include at least the vehicle body, boom, and bucket. Based on the forward kinematics model, calculate the homogeneous transformation matrix between adjacent joints of the loader's working parts; Based on the homogeneous transformation matrix between adjacent joints of the working parts, calculate the homogeneous transformation matrix from the hinge point of the boom and base to the tip of the bucket teeth, and then determine the coordinate equation of the tip of the bucket teeth based on the homogeneous transformation matrix from the hinge point to the tip of the bucket teeth. The method involves using a collision detection algorithm to perform collision detection on the 3D mesh model of the roadway and the bucket based on the bucket's pose relative to the vehicle body coordinate system and the bucket's precise pose in the roadway's global coordinate system. Based on the detection results, a preset safety distance threshold, and joint angle data, collision prediction is performed to obtain the collision prediction results, including: The precise pose of the bucket in the global coordinate system of the tunnel map and the 3D mesh model of the tunnel are coarsely detected using a collision detection library. The coarse detection includes whether the directed bounding box of the bucket intersects with the bounding box of the octree node of the tunnel model. If the detections intersect, the GJK algorithm is used for fine detection, which includes using the GJK algorithm to calculate the minimum distance between the bucket and the roadway grid within the octagonal leaf node. The detection results are obtained based on the minimum distance and the preset safe distance threshold; Differential processing is performed on the joint rotation angle data to obtain real-time angular velocity and real-time angular acceleration; Using a uniform acceleration motion model, we obtain joint angle data, real-time angular velocity, and real-time angular acceleration, and predict joint angle sequences for future moments. Based on the predicted joint angle sequence, perform predicted pose calculation, coarse detection, and fine detection to obtain collision prediction results.

7. An electronic device, characterized in that, Including the processor and memory; The memory is used to store programs; The processor executes the program to implement the loader roadway collision detection and control method as described in any one of claims 1-5.

8. A computer-readable storage medium, characterized in that, The storage medium stores a program, which is executed by a processor to implement the loader roadway collision detection and control method as described in any one of claims 1-5.

Images