Method and device for safe operation control of working machine based on three-dimensional electronic fence

Abstract

The application discloses a kind of based on three-dimensional electronic fence's work machine safety operation control method and device, belong to work machine control technical field.Method includes: based on the actual work environment of work area setting three-dimensional electronic fence;First dynamic parameter of work machine is acquired, the first dynamic parameter includes the first three-dimensional position data and first attitude data when work machine works;According to the first dynamic parameter, the key point coordinate on the work machine is calculated;According to the key point coordinate, the minimum boundary distance of key point and the three-dimensional electronic fence is calculated;According to the minimum boundary distance, safety control measure is determined and is executed.The method can also accurately determine the relative position relationship of the key point of work machine and three-dimensional electronic fence when work machine position changes, without artificial recalibration confirmation, realize work machine dynamic position self-adaptation, avoid misoperation or collision accident, significantly improve construction safety.

Description

Technical Field

[0001] This application relates to the field of machinery control technology, specifically to a method for safe operation control of machinery based on a three-dimensional electronic fence, a device for safe operation control of machinery based on a three-dimensional electronic fence, and a machine-readable storage medium. Background Technology

[0002] An electronic fence (or electronic barrier) is a virtual fencing device used in the operation of machinery. The electronic fence is used to limit the movement position of the machinery's working device. When any part of the working device approaches the boundary of the electronic fence, the machinery controller locks the operating lever of the machinery to avoid safety accidents caused by operator error.

[0003] With the accelerating pace of urbanization and the rapid development of the mining industry, the demand for intelligent machinery is growing rapidly. Excavators, as one of the most widely used construction machines, are facing challenges such as a decrease in the number of operators and the gradual retirement of experienced, aging technicians. Therefore, improving the intelligence level of excavators has become an urgent market need. Currently, various excavator solutions based on two-dimensional electronic fences exist for narrow or complex working environments such as under high-voltage lines, pipe jacking, and tunnel entrances. Excavator electronic fences primarily define virtual areas for excavators, restricting the machine's movement range on a two-dimensional working surface, dynamically monitoring key points, issuing alarms when approaching boundaries, and automatically stopping current operations when crossing boundaries. However, their applicability remains insufficient in mining or confined space construction. Summary of the Invention

[0004] The purpose of this application is to provide a method and device for safe operation control of machinery based on a three-dimensional electronic fence. This method sets up a three-dimensional electronic fence based on the actual working environment, accurately covering the three-dimensional direction. It determines the coordinates of the key points of the machinery based on the dynamic parameters of the machinery, thereby determining the minimum boundary distance between the key points and the three-dimensional electronic fence. Even when the position of the machinery changes, it can accurately determine the relative positional relationship between the key points of the machinery and the three-dimensional electronic fence without the need for manual recalibration and confirmation, thus realizing the dynamic position self-adaptation of the machinery.

[0005] To achieve the above objectives, the first aspect of this application provides a method for safe operation control of machinery based on a three-dimensional electronic fence, the method comprising: A three-dimensional electronic fence is set up based on the actual working environment of the work area; Acquire the first dynamic parameters of the operating machinery, the first dynamic parameters including the first three-dimensional position data and the first posture data of the operating machinery during operation; Calculate the coordinates of key points on the operating machinery based on the first dynamic parameters; Calculate the minimum boundary distance between the key point and the three-dimensional electronic fence based on the key point coordinates; Safety control measures are determined and implemented based on the minimum boundary distance.

[0006] Based on the above technical means, this method sets up a three-dimensional electronic fence based on the actual working environment, accurately covering the three-dimensional direction. It determines the coordinates of the key points of the working machinery based on the dynamic parameters of the working machinery, thereby determining the minimum boundary distance between the key points and the three-dimensional electronic fence. When the position of the working machinery changes, it can also accurately determine the relative position relationship between the key points of the working machinery and the three-dimensional electronic fence, without the need for manual recalibration and confirmation. It realizes the dynamic position self-adaptation of the working machinery, avoids misoperation or collision accidents, and significantly improves construction safety.

[0007] In some feasible embodiments, a three-dimensional electronic fence is set up based on the actual working environment of the work area, including: The second dynamic parameter is obtained when the operating machinery is calibrated at each calibration point. The calibration point is a position point in the actual working environment of the working area. The second dynamic parameter includes second three-dimensional position data and second attitude data. Calculate the world coordinates of the calibration point based on the second dynamic parameter; A closed three-dimensional electronic fence is formed based on the world coordinates of the calibration points.

[0008] Based on the above technical means, calibration points in the actual working environment are calibrated using operating machinery, and then a closed three-dimensional electronic fence is formed based on the world coordinates of the calibration points. The three-dimensional electronic fence formed in this way can conform to the actual terrain. For irregular areas, the formed three-dimensional electronic fence can better reduce the risk of accidental collision.

[0009] In some feasible embodiments, calculating the world coordinates of the calibration point based on the second dynamic parameter includes: Based on the second posture data of the working machinery and the geometric structure of the working machinery, determine the first coordinate transformation matrix from the coordinate origin of the working machinery to the contact position between the working machinery and the calibration point; The world coordinates of the calibration point are calculated based on the second three-dimensional position data of the operating machinery and the first coordinate transformation matrix.

[0010] Based on the aforementioned technical means, the first coordinate transformation matrix from the origin of the work machinery to the contact position between the work machinery and the calibration point can be determined by utilizing the inherent parameters of the work machinery. Based on this first coordinate transformation matrix and the current second three-dimensional position data of the work machinery, the world coordinates of the calibration point can be obtained. This method utilizes the integrated navigation module deployed on the work machinery to quickly calculate the world coordinates of the calibration point, eliminating the need for users to determine the coordinates of the three-dimensional electronic fence through additional positioning equipment, thus saving costs and fence setup time.

[0011] In some feasible embodiments, calculating the coordinates of key points on the operating machinery based on the first dynamic parameter includes: Based on the first posture data of the working machinery and the geometric structure of the working machinery, determine the second coordinate transformation matrix from the coordinate origin of the working machinery to the key point; The world coordinates of the key points are calculated based on the first three-dimensional position data of the operating machinery and the second coordinate transformation matrix.

[0012] Based on the aforementioned technical means, the second coordinate transformation matrix from the origin of the machine's coordinates to the key point can be determined using the inherent parameters of the machine. Based on this second coordinate transformation matrix and the current first three-dimensional position data of the machine, the world coordinates of the key point can be obtained. This method utilizes the integrated navigation module deployed on the machine, which can quickly calculate the world coordinates of the key point. Even if the dynamic parameters of the machine change, the world coordinates of the key point can still be quickly calculated without repeated manual calibration, thereby greatly improving work efficiency.

[0013] In some feasible embodiments, calculating the minimum boundary distance between the key point and the three-dimensional electronic fence based on the key point coordinates includes: When the working machinery is determined to be stationary and in a fixed direction based on the first posture data, the minimum boundary distance between the key point and the three-dimensional electronic fence is calculated using the point-to-cube distance calculation formula based on the coordinates of the key point.

[0014] In some feasible embodiments, calculating the minimum boundary distance between the key point and the three-dimensional electronic fence based on the key point coordinates includes: When the working machinery tilts based on the first posture data, the minimum boundary distance between the key point and the three-dimensional electronic fence is calculated using the point-to-triangle distance calculation formula based on the coordinates of the key point.

[0015] Based on the aforementioned technical means, the minimum boundary distance between key points and the three-dimensional electronic fence can be calculated in different ways depending on whether the operating machinery is tilted. This simplifies the calculation method, ensures the accuracy of the calculation results, and significantly improves construction safety.

[0016] In some feasible embodiments, the method further includes: Predict the future path segment of the operating machinery based on the first dynamic parameters; Calculate the minimum predicted boundary distance between the key point and the three-dimensional electronic fence for each predicted point based on the future path segment; Pre-control measures are determined and implemented based on the minimum prediction boundary distance and the time corresponding to the prediction point.

[0017] Based on the above-mentioned technical means, the future path segment of the operating machinery can be predicted, and control can be carried out in advance according to the minimum predicted boundary distance to avoid safety hazards caused by insufficient emergency braking buffer distance of the working device.

[0018] In some feasible embodiments, predicting the future path segment of the operating machinery based on the first dynamic parameter includes: Predict the future attitude data and future three-dimensional position data of the operating machinery based on the first dynamic parameter and the set time interval; Based on the future attitude data and the geometry of the working machine, determine the third coordinate transformation matrix from the origin of the working machine coordinate system to the key point; The future world coordinates of the key points are calculated based on the future 3D location data and the third coordinate transformation matrix. The future world coordinates of the same key point at different times form the future path segments corresponding to the key points.

[0019] A second aspect of this application provides a safety operation control device for machinery based on a three-dimensional electronic fence, the device comprising: Fence setting unit, used to set up a three-dimensional electronic fence based on the actual terrain of the work area; The data acquisition unit is used to acquire the first dynamic parameters of the operating machinery, the first dynamic parameters including the first three-dimensional position data and the first posture data of the operating machinery; A coordinate transformation unit is used to calculate the coordinates of key points on the operating machinery based on the first dynamic parameters; A distance calculation unit is used to calculate the minimum boundary distance between the key point and the three-dimensional electronic fence based on the coordinates of the key point; An execution unit is used to determine and execute safety control measures based on the minimum boundary distance.

[0020] Based on the aforementioned technical means, this device sets up a three-dimensional electronic fence based on the actual working environment, accurately covering the three-dimensional direction. It determines the coordinates of the key points of the working machinery based on the dynamic parameters of the working machinery, thereby determining the minimum boundary distance between the key points and the three-dimensional electronic fence. Even when the position of the working machinery changes, it can accurately determine the relative positional relationship between the key points of the working machinery and the three-dimensional electronic fence, without the need for manual recalibration and confirmation. This enables the dynamic position of the working machinery to adapt, avoids misoperation or collision accidents, and significantly improves construction safety.

[0021] A third aspect of this application provides a machine-readable storage medium storing instructions for causing a machine to execute the aforementioned method for safe operation control of machinery based on a three-dimensional electronic fence.

[0022] The above technical solution provides a method for safe operation control of machinery based on a three-dimensional electronic fence. This method sets up a three-dimensional electronic fence based on the actual working environment, accurately covering the three-dimensional direction. The coordinates of the key points of the machinery are determined based on the dynamic parameters of the machinery, thereby determining the minimum boundary distance between the key points and the three-dimensional electronic fence. Even when the position of the machinery changes, the relative positional relationship between the key points of the machinery and the three-dimensional electronic fence can be accurately determined without manual recalibration and confirmation, realizing dynamic position self-adaptation of the machinery; avoiding misoperation or collision accidents, and significantly improving construction safety.

[0023] Other features and advantages of the embodiments of this application will be described in detail in the following detailed description section. Attached Figure Description

[0024] The accompanying drawings are provided to further illustrate the embodiments of this application and form part of the specification. They are used together with the following detailed description to explain the embodiments of this application, but do not constitute a limitation on the embodiments of this application. In the drawings: Figure 1 A schematic diagram illustrating the topology of a work machinery system according to an embodiment of this application is shown. Figure 2 This illustration schematically shows a step diagram of a method for controlling the safe operation of machinery based on a three-dimensional electronic fence according to an embodiment of this application; Figure 3 A schematic diagram illustrating the overall modeling of an excavator according to an embodiment of this application is shown. Figure 4 A schematic diagram of the fence is shown when the excavator moves and tilts according to an embodiment of this application; Figure 5 This schematic diagram illustrates the two-dimensional fence rotation restriction area and the ideal area of ​​an excavator according to an embodiment of this application; Figure 6This illustration schematically shows a step diagram of another method for safe operation control of machinery based on a three-dimensional electronic fence according to an embodiment of this application; Figure 7 A schematic diagram illustrating the overall control flow according to the method of this application is shown. Detailed Implementation

[0025] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are only for illustration and explanation of the embodiments of this application and are not intended to limit the embodiments of this application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without creative effort are within the scope of protection of this application.

[0026] It should be noted that the acquisition, transmission, storage, use, and processing of data in the technical solution of this application all comply with relevant laws and regulations. In the embodiments of this application, certain existing industry solutions such as software, components, and models may be mentioned. These should be considered exemplary, intended only to illustrate the feasibility of implementing the technical solution of this application, and do not imply that the applicant has already used or necessarily used such solutions.

[0027] It should be noted that if the embodiments of this application involve directional indicators (such as up, down, left, right, front, back, etc.), the directional indicators are only used to explain the relative positional relationship and movement of each component in a certain specific posture (as shown in the figure). If the specific posture changes, the directional indicators will also change accordingly.

[0028] Furthermore, if the embodiments of this application involve descriptions such as "first" or "second," these descriptions are for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of technical features indicated. Therefore, features defined with "first" or "second" may explicitly or implicitly include at least one of those features. Additionally, the technical solutions of various embodiments can be combined with each other, but this must be based on the ability of those skilled in the art to implement them. If the combination of technical solutions is contradictory or impossible to implement, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed in this application.

[0029] Existing electronic fences have the following drawbacks: 1. It can only define planar boundaries, resulting in limited spatial constraints. Specifically, it restricts only one dimension: it can usually only define one or more horizontal and vertical boundaries, such as height and depth limits; furthermore, it cannot form a complete closed spatial region, cannot effectively define a three-dimensional restricted space, and can only restrict some directions.

[0030] 2. Weak adaptability to complex environments. Specifically, in sites with complex spatial structures such as tunnels, bridges, and irregularly shaped buildings, two-dimensional boundaries alone cannot accurately cover all directions. For example, on sloping ground or multi-layered steep slopes, dynamic protection cannot be provided in accordance with the actual terrain.

[0031] 3. Lack of dynamic attitude adaptation capability. Traditional two-dimensional fences are mostly statically set up and do not have the ability to dynamically adjust the boundary according to the real-time attitude of the operating machinery. Once the position or attitude of the operating machinery changes, manual recalibration is required, resulting in low work efficiency and difficulty in responding to the operating status in a timely manner.

[0032] To address the aforementioned technical issues, this application proposes a method for safe operation control of machinery based on a three-dimensional electronic fence. This method utilizes the machinery's own navigation and positioning system to acquire its three-dimensional position and attitude in a world coordinate system. Within the machinery's (e.g., an excavator's) operating space, a fully enclosed or semi-enclosed three-dimensional electronic fence is defined using the three-dimensional coordinate system. With the support of the navigation and positioning system, the machinery can sense the spatial position of obstacles relative to itself even when it moves or changes its attitude. The system calculates the position and attitude of the machinery's end effector (e.g., bucket) or other critical components in real time. When these components are about to touch the virtual boundary, an automatic warning is issued, and safety control measures such as speed limits or emergency stops can be implemented according to settings.

[0033] like Figure 1The diagram illustrates the topology of the operating machinery system described in this application. The system includes a main controller, instruments, a data acquisition unit, and an execution unit. The data acquisition unit collects dynamic parameters of the operating machinery, including a real-time kinematic (RTK) module, an inertial measurement unit (IMU), pressure sensors, and tilt sensors. The execution unit controls valve core current, pump displacement, and engine power. The main controller determines the coordinates of key points on the operating machinery in real time based on the first dynamic parameters collected by the data acquisition unit, then calculates the minimum boundary distance between the key points and the three-dimensional electronic fence. Based on the minimum boundary distance, it determines whether the operating machinery is about to touch the boundary of the three-dimensional electronic fence. When it is about to touch the boundary, safety control measures are implemented. The main controller includes, but is not limited to, computing units such as microcontroller-based, CPU-based industrial control computers, and CPU+TPU processor-based systems; the real-time dynamic positioning module can be replaced by positioning units such as Global Navigation Satellite System (GNSS), Precise Point Positioning (PPP), and Satellite-Based Augmentation System (SBAS); and the inertial measurement unit can be replaced by Visual-Inertial Odometry (VIO), Doppler radar, lidar, and binocular depth camera.

[0034] Figure 2 The illustration schematically depicts the steps of a method for controlling the safe operation of machinery based on a three-dimensional electronic fence, according to an embodiment of this application. For example... Figure 2 As shown, the method may include the following steps.

[0035] S1: Set up a three-dimensional electronic fence based on the actual working environment of the work area.

[0036] In some feasible embodiments, a three-dimensional electronic fence is set up based on the actual working environment of the work area, including: S101: Obtain the second dynamic parameters of the operating machinery when calibrating various calibration points. The calibration points are location points in the actual working environment of the operating area. The second dynamic parameters include second three-dimensional position data and second attitude data. Taking an excavator as an example, when setting up a three-dimensional electronic fence, the operator can manipulate the excavator to contact the tooth tip with the location point in the actual working environment, and record the three-dimensional position data and attitude data of the excavator at the time of contact as dynamic parameters. Among them, the three-dimensional position data mainly includes the coordinates of the operating machinery in the world coordinate system, and the attitude data mainly includes the tilt angle, slewing angle, pitch angle, roll angle, etc. of the operating machinery. For ease of distinction, the dynamic parameters during the calibration process are recorded as the second dynamic parameters, the three-dimensional position data as the second three-dimensional position data, and the attitude data as the second attitude data.

[0037] S102: Then, the world coordinates of the calibration point are calculated based on the second dynamic parameter, specifically including: Based on the second posture data of the working machinery and its geometry, a first coordinate transformation matrix is ​​determined from the origin of the working machinery's coordinate system to the contact position between the working machinery and the calibration point. In this embodiment, the first coordinate transformation matrix refers to the transformation matrix from the origin of the working machinery's coordinate system to the contact position between the working machinery and the calibration point.

[0038] In this embodiment of the application, the real-time joint angle values ​​of the excavator boom, stick, and bucket can be calculated based on the tilt angle in the second attitude data. For example... Figure 3 A schematic diagram of the excavator's overall model is shown, illustrating the excavator's geometric structure model, including the boom joint angle values. Directly from boom tilt angle The measured value is . Stick joint angle value For the angle of the boom With boom tilt angle The difference is The bucket joint angle value needs to be obtained through some geometric calculations. Specifically, it involves converting the tilt angle mounted on the GI link into the angle between HL and the horizontal line. Then, by combining the boom and stick tilt angles, we can obtain... .

[0039] After obtaining the joint angle, transforming the rotation center coordinate system Q to the boom hinge point O yields: ; in, For the rotation joint angle, Let O be the length of the link connecting point O in the boom hinge coordinate system and point Q in the rotation center coordinate system. Let O be the joint distance between point O in the boom hinge coordinate system and point Q in the rotation center coordinate system. The distance between point O in the boom hinge coordinate system and point Q in the slewing center coordinate system is the link offset distance.

[0040] The transformation matrix from the boom hinge coordinate system O to the stick hinge coordinate system E is: ; in, Let O be the length of the link from point O in the boom hinge coordinate system to point E in the stick hinge. Let O be the joint distance from point O in the boom hinge coordinate system to point E in the stick hinge.

[0041] The transformation matrix from the boom hinge coordinate system E to the bucket hinge coordinate system H is: ; in, Let H be the length of the link connecting point E in the boom hinge coordinate system to the bucket hinge point H. Let E be the joint distance from point E in the boom hinge coordinate system to point H in the stick hinge.

[0042] The transformation matrix from the bucket hinge coordinate system H to the bucket tooth tip coordinate system L is: ; in, Let H be the length of the link from point H in the bucket hinge coordinate system to the tip L of the bucket tooth. Let H be the joint length from point H in the bucket hinge coordinate system to the tip L of the bucket tooth.

[0043] The transformation matrix from the rotation center coordinate system Q to the bucket tooth tip coordinate system L is: ; in, , , , .

[0044] A slewing encoder installed at the junction of the slewing and lower slewing sections on the excavator body provides the slewing angle. A tilt sensor installed on the upper slewing section, after calibration, provides the pitch and roll angles. By performing forward kinematic analysis on these angles and the excavator's structural parameters, the coordinate transformation matrix of the excavator body relative to the ground is derived.

[0045] In some embodiments, calibration is performed by having the excavator tooth tip in contact with the actual environment of the working area. The transformation matrix from the rotation center coordinate system Q to the bucket tooth tip coordinate system L can be used as the first coordinate transformation matrix. If calibration is performed by having other parts of the excavator in contact with the actual environment of the working area, the transformation matrix can be derived by combining geometric principles with the whole machine modeling diagram.

[0046] The position of the bucket tip is represented as follows: Expanding this gives us: ; A slewing encoder installed at the junction of the slewing and lower slewing sections on the excavator body provides the slewing angle. A tilt sensor installed on the upper slewing section, after calibration, provides the pitch and roll angles. By performing forward kinematic analysis on these angles and the excavator's structural parameters, the coordinate transformation matrix of the excavator body relative to the ground is derived.

[0047] Then, the world coordinates of the calibration point are calculated based on the second and third-dimensional position data of the operating machinery and the first coordinate transformation matrix. In this way, the first coordinate transformation matrix from the origin of the operating machinery's coordinates to the contact position between the operating machinery and the calibration point can be determined using the inherent parameters of the operating machinery. Based on this first coordinate transformation matrix and the current second and third-dimensional position data of the operating machinery, the world coordinates of the calibration point can be obtained. This method utilizes the integrated navigation module deployed on the operating machinery, which can quickly calculate the world coordinates of the calibration point without requiring users to determine the coordinates of the three-dimensional electronic fence through additional positioning equipment, saving costs and fence setup time.

[0048] S103: A closed three-dimensional electronic fence is formed based on the world coordinates of the calibration point. In some feasible embodiments, when forming the closed three-dimensional electronic fence, the fence is extended away from the obstacle by a preset distance to reserve sufficient space to avoid the obstacle. In one feasible embodiment, the preset distance is 5-10cm.

[0049] In quarries, ponds, tunnels, and other similar working conditions, the boundary constraints are often polygonal or arc-shaped. Simply relying on horizontal or vertical planar boundaries cannot accurately reflect the actual terrain. This application utilizes machinery to calibrate points in the actual working environment, and then forms a closed three-dimensional electronic fence based on the world coordinates of these points. This method creates a three-dimensional electronic fence that conforms to the actual terrain, offering greater adaptability. It is not limited to planar boundaries but can also flexibly define various obstacle boundaries such as slopes, curved surfaces, and complex structures. For irregular areas such as quarries, ponds, and tunnels, the resulting three-dimensional electronic fence can better reduce the risk of accidental collisions. Figure 5 The diagram shows the two-dimensional enclosure of the excavator, which restricts the slewing area and creates an ideal area. The restricted slewing area forms a cubic region with a fan-shaped base based on the slewing angle, while the ideal area is a cubic region with a quadrilateral base.

[0050] In other feasible embodiments, a three-dimensional electronic fence can also be set manually, that is, the center and starting point of the arc are determined manually, the circular area is drawn by drawing the arc, and finally the instrument is clicked to allow or prohibit entry into the area to achieve protection.

[0051] S2: Obtain the first dynamic parameters of the operating machinery. The first dynamic parameters include the first three-dimensional position data and the first posture data of the operating machinery during operation. In this embodiment, the first dynamic parameters are the dynamic parameters of the operating machinery during the working process. For ease of distinction, the dynamic parameters of the operating machinery during the working process are referred to as the first dynamic parameters, the three-dimensional position data in the first dynamic parameters are referred to as the first three-dimensional position data, and the posture data are referred to as the first posture data.

[0052] S3: Calculate the coordinates of key points on the operating machinery based on the first dynamic parameters.

[0053] In some feasible embodiments, calculating the coordinates of key points on the operating machinery based on the first dynamic parameter includes: Based on the first posture data of the working machine and the geometric structure of the working machine, the second coordinate transformation matrix from the coordinate origin of the working machine to the key point is determined. In the embodiment of this application, each key point corresponds to a coordinate transformation matrix. The method of determining the coordinate transformation matrix is ​​the same as the method of determining the first coordinate transformation matrix in the calibration process. Both are determined by combining the determined joint angle values ​​and the structural parameters of the working machine itself to perform forward kinematic analysis. This will not be elaborated here.

[0054] The world coordinates of the key points are calculated based on the first three-dimensional position data of the operating machinery and the second coordinate transformation matrix.

[0055] Based on the aforementioned technical means, the second coordinate transformation matrix from the origin of the machine's coordinates to the key point can be determined using the inherent parameters of the machine. Based on this second coordinate transformation matrix and the current first three-dimensional position data of the machine, the world coordinates of the key point can be obtained. This method utilizes the integrated navigation module deployed on the machine, which can quickly calculate the world coordinates of the key point. Even if the dynamic parameters of the machine change, the world coordinates of the key point can still be quickly calculated without repeated manual calibration, thereby greatly improving work efficiency.

[0056] S4: Calculate the minimum boundary distance between the key point and the three-dimensional electronic fence based on the key point coordinates. For example... Figure 4 As shown, during operation, the distance between the working machinery and the three-dimensional electronic fence will change after the machinery is moved or tilted. Based on the coordinates of key points, the minimum distance between the working machinery and the three-dimensional electronic fence can be calculated, thereby determining whether the working machinery will collide with obstacles and taking safety control measures.

[0057] In some feasible embodiments, calculating the minimum boundary distance between the key point and the three-dimensional electronic fence based on the key point coordinates includes: When the operating machinery remains stationary and its orientation is fixed based on the first posture data, the minimum boundary distance between the key point and the three-dimensional electronic fence is calculated using the point-to-cube distance calculation formula based on the key point coordinates. The point-to-cube distance calculation formula is as follows: ; in, For the coordinates of the key points, Let be a point on the cube. , These are the minimum and maximum values ​​of the cube along the x, y, and z axes, respectively. In practice... These are points on a three-dimensional electronic fence. Assuming the key point coordinates are (4,6,2), and the cube has a side length of 3, the coordinates of its six vertices are: (3,0,0), (0,3,0), (0,0,3), (3,3,0), (3,0,3), (0,3,3), (3,3,3), (0,0,0). Based on these coordinates, we can determine that the minimum value on the cube's y-axis is 0, and the maximum value is 3; the minimum value on the cube's x-axis is 0, and the maximum value is 3; and the minimum value on the cube's z-axis is 0, and the maximum value is 3. Using the formula above, we can calculate... hour, , hour, , hour, The calculation results for each axis are then compared with 0 to obtain the maximum value. .

[0058] In some other feasible embodiments, calculating the minimum boundary distance between the key point and the three-dimensional electronic fence based on the key point coordinates includes: When the working machinery tilts based on the first posture data, the minimum boundary distance between the key point and the three-dimensional electronic fence is calculated using the point-to-triangle distance calculation formula based on the coordinates of the key point.

[0059] Based on the aforementioned technical means, the minimum boundary distance between key points and the three-dimensional electronic fence can be calculated in different ways depending on whether the operating machinery is tilted. This simplifies the calculation method, ensures the accuracy of the calculation results, and significantly improves construction safety.

[0060] S5: Determine and execute safety control measures based on the minimum boundary distance. Specific safety control measures can be set according to safety requirements. For example, the safety control measures can be set as follows: when the minimum boundary distance is less than a first preset distance but greater than or equal to a second preset distance, issue a level one warning and implement speed limits; when the minimum boundary distance is less than the second preset distance, issue a level two warning and implement an economical stop.

[0061] Based on the above technical means, this method sets up a three-dimensional electronic fence based on the actual working environment, accurately covering the three-dimensional direction. It determines the coordinates of the key points of the working machinery based on the dynamic parameters of the working machinery, thereby determining the minimum boundary distance between the key points and the three-dimensional electronic fence. When the position of the working machinery changes, it can also accurately determine the relative position relationship between the key points of the working machinery and the three-dimensional electronic fence, without the need for manual recalibration and confirmation. It realizes the dynamic position self-adaptation of the working machinery, avoids misoperation or collision accidents, and significantly improves construction safety.

[0062] In some feasible embodiments, such as Figure 6 As shown, the method further includes: S6: Predict the future path segment of the operating machinery based on the first dynamic parameters. Specifically, this includes: The future attitude data and future three-dimensional position data of the operating machinery are predicted based on the first dynamic parameter and the set time interval.

[0063] In this embodiment, future actions are predicted using the current joint angles (boom, stick, bucket) and their angular velocities. The state-space equation simplifies to: ; in, For time intervals, Let be the angle at time t. It represents angular velocity.

[0064] Based on the future attitude data and the geometry of the working machine, determine the third coordinate transformation matrix from the origin of the working machine coordinate system to the key point; The future world coordinates of the key points are calculated based on the future 3D location data and the third coordinate transformation matrix. The future world coordinates of the same key point at different times form the future path segments corresponding to the key points.

[0065] Based on the aforementioned overall kinematics, calculate the positions of key points in world coordinates at future moments. This allows for the prediction of tooth tip positions or critical components, continuously predicting n future time points to form path segments: .

[0066] S7: Calculate the minimum predicted boundary distance between the key point and the three-dimensional electronic fence in each predicted point according to the future path segment. The calculation method of the minimum predicted boundary distance is the same as the aforementioned minimum boundary distance calculation method.

[0067] S8: Determine and execute advance control measures based on the minimum prediction boundary distance and the time corresponding to the prediction point.

[0068] Advance control measures could include imposing speed limits a certain time in advance, based on the predicted time, when the minimum predicted boundary distance is expected to reach the Level 1 warning and speed limit conditions at a future time. For example, if the current time is 15:00:00 and the prediction is that the minimum predicted boundary distance will reach the Level 1 warning and speed limit conditions at 15:01:10, then the warning and speed limit would be implemented at 15:01:05. Other safety control measures would be set sequentially.

[0069] Based on the above-mentioned technical means, the future path segment of the operating machinery can be predicted, and control can be carried out in advance according to the minimum predicted boundary distance to avoid safety hazards caused by insufficient emergency braking buffer distance of the working device.

[0070] like Figure 7 The overall control flow diagram of the method of this application is shown. The method of this application establishes the boundary of the three-dimensional electronic fence through calibration. During the operation of the operating machinery, multiple sensors collect second dynamic parameters and input them into the main controller. The main controller performs attitude / position fusion and calculates the spatial position of each moving part on the operating machinery. On the one hand, it uses the spatial position of each moving part on the operating machinery to determine the closest distance with the three-dimensional boundary of the three-dimensional electronic fence, makes real-time decision output, generates corresponding control commands, and controls the hydraulic actuator to execute. On the other hand, it predicts future path segments based on the calculated spatial positions, and then predicts whether the future path segments will exceed the boundary of the three-dimensional electronic fence. Based on the boundary prediction results, it makes decision output, generates corresponding control commands, and controls the hydraulic actuator to execute.

[0071] A second aspect of this application provides a safety operation control device for machinery based on a three-dimensional electronic fence, the device comprising: Fence setting unit, used to set up a three-dimensional electronic fence based on the actual terrain of the work area; The data acquisition unit is used to acquire the first dynamic parameters of the operating machinery, the first dynamic parameters including the first three-dimensional position data and the first posture data of the operating machinery; A coordinate transformation unit is used to calculate the coordinates of key points on the operating machinery based on the first dynamic parameters; A distance calculation unit is used to calculate the minimum boundary distance between the key point and the three-dimensional electronic fence based on the coordinates of the key point; An execution unit is used to determine and execute safety control measures based on the minimum boundary distance.

[0072] Based on the aforementioned technical means, this device sets up a three-dimensional electronic fence based on the actual working environment, accurately covering the three-dimensional direction. It determines the coordinates of the key points of the working machinery based on the dynamic parameters of the working machinery, thereby determining the minimum boundary distance between the key points and the three-dimensional electronic fence. Even when the position of the working machinery changes, it can accurately determine the relative positional relationship between the key points of the working machinery and the three-dimensional electronic fence, without the need for manual recalibration and confirmation. This enables the dynamic position of the working machinery to adapt, avoids misoperation or collision accidents, and significantly improves construction safety.

[0073] A third aspect of this application provides a machine-readable storage medium storing instructions for causing a machine to execute the aforementioned method for safe operation control of machinery based on a three-dimensional electronic fence.

[0074] The above technical solution provides a method for safe operation control of machinery based on a three-dimensional electronic fence. This method sets up a three-dimensional electronic fence based on the actual working environment, accurately covering the three-dimensional direction. The coordinates of the key points of the machinery are determined based on the dynamic parameters of the machinery, thereby determining the minimum boundary distance between the key points and the three-dimensional electronic fence. Even when the position of the machinery changes, the relative positional relationship between the key points of the machinery and the three-dimensional electronic fence can be accurately determined without manual recalibration and confirmation. This enables the machinery to dynamically adapt its position, avoids misoperation or collision accidents, and significantly improves construction safety.

[0075] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0076] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0077] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0078] These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable apparatus for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0079] In a typical configuration, a computing device includes one or more processors (CPU), input / output interfaces, network interfaces, and memory.

[0080] Memory may include non-persistent memory in computer-readable media, such as random access memory (RAM) and / or non-volatile memory, such as read-only memory (ROM) or flash RAM. Memory is an example of computer-readable media.

[0081] Computer-readable media includes both permanent and non-permanent, removable and non-removable media that can store information using any method or technology. Information can be computer-readable instructions, data structures, modules of programs, or other data. Examples of computer storage media include, but are not limited to, phase-change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technologies, CD-ROM, digital versatile optical disc (DVD) or other optical storage, magnetic tape, disk storage or other magnetic storage devices, or any other non-transferable medium that can be used to store information accessible by a computing device. As defined herein, computer-readable media does not include transient computer-readable media, such as modulated data signals and carrier waves.

[0082] It should also be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes that element.

[0083] The above are merely embodiments of this application and are not intended to limit the scope of this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the scope of the claims of this application.

Claims

1. A method for safe operation control of machinery based on a three-dimensional electronic fence, characterized in that, The method includes: A three-dimensional electronic fence is set up based on the actual working environment of the work area; Acquire the first dynamic parameters of the operating machinery, the first dynamic parameters including the first three-dimensional position data and the first posture data of the operating machinery during operation; Calculate the coordinates of key points on the operating machinery based on the first dynamic parameters; Calculate the minimum boundary distance between the key point and the three-dimensional electronic fence based on the key point coordinates; Safety control measures are determined and implemented based on the minimum boundary distance; Predict the future path segment of the operating machinery based on the first dynamic parameters; Calculate the minimum predicted boundary distance between the key point and the three-dimensional electronic fence for each predicted point based on the future path segment; Based on the minimum predicted boundary distance and the time corresponding to the predicted point, determine and implement advance control measures; The step of predicting the future path segment of the operating machinery based on the first dynamic parameter includes: Predict the future attitude data and future three-dimensional position data of the operating machinery based on the first dynamic parameter and the set time interval; Based on the future attitude data and the geometry of the working machine, determine the third coordinate transformation matrix from the origin of the working machine coordinate system to the key point; The future world coordinates of the key points are calculated based on the future 3D location data and the third coordinate transformation matrix. The future world coordinates of the same key point at different times form the future path segments corresponding to the key points.

2. The method for safe operation control of machinery based on a three-dimensional electronic fence according to claim 1, characterized in that, Based on the actual working environment of the work area, a three-dimensional electronic fence is set up, including: The second dynamic parameter is obtained when the operating machinery is calibrated at each calibration point. The calibration point is a position point in the actual working environment of the working area. The second dynamic parameter includes second three-dimensional position data and second attitude data. Calculate the world coordinates of the calibration point based on the second dynamic parameter; A closed three-dimensional electronic fence is formed based on the world coordinates of the calibration points.

3. The method for safe operation control of machinery based on a three-dimensional electronic fence according to claim 2, characterized in that, The world coordinates of the calibration point are calculated based on the second dynamic parameter, including: Based on the second posture data of the working machinery and the geometric structure of the working machinery, determine the first coordinate transformation matrix from the coordinate origin of the working machinery to the contact position between the working machinery and the calibration point; The world coordinates of the calibration point are calculated based on the second three-dimensional position data of the operating machinery and the first coordinate transformation matrix.

4. The method for safe operation control of machinery based on a three-dimensional electronic fence according to claim 1, characterized in that, Calculating the coordinates of key points on the operating machinery based on the first dynamic parameter includes: Based on the first posture data of the working machinery and the geometric structure of the working machinery, determine the second coordinate transformation matrix from the coordinate origin of the working machinery to the key point; The world coordinates of the key points are calculated based on the first three-dimensional position data of the operating machinery and the second coordinate transformation matrix.

5. The method for safe operation control of machinery based on a three-dimensional electronic fence according to claim 1, characterized in that, Calculating the minimum boundary distance between the key point and the three-dimensional electronic fence based on the key point coordinates includes: When the working machinery is determined to be stationary and in a fixed direction based on the first posture data, the minimum boundary distance between the key point and the three-dimensional electronic fence is calculated using the point-to-cube distance calculation formula based on the coordinates of the key point.

6. The method for safe operation control of machinery based on a three-dimensional electronic fence according to claim 1, characterized in that, Calculating the minimum boundary distance between the key point and the three-dimensional electronic fence based on the key point coordinates includes: When the working machinery tilts based on the first posture data, the minimum boundary distance between the key point and the three-dimensional electronic fence is calculated using the point-to-triangle distance calculation formula based on the coordinates of the key point.

7. A safety operation control device for machinery based on a three-dimensional electronic fence, used to implement the safety operation control method for machinery based on a three-dimensional electronic fence as described in any one of claims 1-6, characterized in that, The device includes: Fence setting unit, used to set up a three-dimensional electronic fence based on the actual terrain of the work area; The data acquisition unit is used to acquire the first dynamic parameters of the operating machinery, the first dynamic parameters including the first three-dimensional position data and the first posture data of the operating machinery; A coordinate transformation unit is used to calculate the coordinates of key points on the operating machinery based on the first dynamic parameters; A distance calculation unit is used to calculate the minimum boundary distance between the key point and the three-dimensional electronic fence based on the coordinates of the key point; An execution unit is used to determine and execute safety control measures based on the minimum boundary distance.

8. A machine-readable storage medium, characterized in that, The machine-readable storage medium stores instructions for causing the machine to perform the safety operation control method for operating machinery based on a three-dimensional electronic fence as described in any one of claims 1 to 6.

Images