Method and device for controlling safe operation of operation machinery based on three-dimensional electronic fence

Abstract

The invention discloses an operation machine safe operation control method and device based on a three-dimensional electronic fence, and belongs to the technical field of operation machine control. The method comprises the steps of setting a three-dimensional electronic fence based on an actual working environment of a working area; acquiring a first dynamic parameter of the working machine, wherein the first dynamic parameter comprises first three-dimensional position data and first attitude data when the working machine works; calculating a key point coordinate on the operation machine according to the first dynamic parameter; calculating the minimum boundary distance between the key point and the three-dimensional electronic fence according to the coordinates of the key point; and determining and executing a safety control measure according to the minimum boundary distance. According to the method, the relative position relation between the key point of the operation machine and the three-dimensional electronic fence can be accurately determined when the position of the operation machine changes, manual recalibration and confirmation are not needed, dynamic position self-adaption of the operation machine is achieved, misoperation or collision accidents are avoided, and construction safety is remarkably improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of work machine control, in particular to a work machine safety operation control method based on a three-dimensional electronic fence, a work machine safety operation control device based on a three-dimensional electronic fence and a machine readable storage medium. BACKGROUND

[0002] An electronic fence (also known as an electronic fence) is a virtual fence device applied in a work machine operation scene. The electronic fence is used to limit the movement position of the work device of the work machine. When the work device is at any position close to the boundary of the electronic fence, the controller of the work machine locks the operating lever of the work machine to avoid safety accidents caused by the operator's mistake.

[0003] With the continuous acceleration of urbanization and the rapid development of the mining industry, there is an increasing demand for work machines with intelligent functions. As one of the most widely used engineering machines, excavators are facing the challenges of a decreasing number of operators and experienced aging technicians gradually withdrawing from the market. Therefore, it is an urgent need to improve the intelligent level of excavators. At present, in narrow or complex operation environments such as high-voltage lines, pipe jacking and tunnel entrances, there are many excavator solutions based on two-dimensional electronic fences. The current excavator electronic fence mainly limits the virtual area of the excavator, limits the movement range of the machine body in the two-dimensional working surface, dynamically monitors the key points, alarms when approaching the boundary, and automatically stops the current operation action when crossing the boundary. However, in mine exploitation or limited space construction, there are still problems of insufficient applicability. SUMMARY

[0004] The purpose of the embodiments of the present application is to provide a work machine safety operation control method and device based on a three-dimensional electronic fence. The method sets a three-dimensional electronic fence based on the actual operation environment, accurately covers three-dimensional directions, determines the key point coordinates of the work machine based on the dynamic parameters of the work machine, determines the minimum boundary distance between the key points and the three-dimensional electronic fence, accurately determines the relative positional relationship between the key points of the work machine and the three-dimensional electronic fence when the position of the work machine changes, does not need to be manually recalibrated and confirmed, and realizes the self-adaptation of the dynamic position of the work machine.

[0005] To achieve the above purpose, the first aspect of the present application provides a work machine safety operation control method based on a three-dimensional electronic fence, which comprises: setting a three-dimensional electronic fence based on the actual operation environment of the operation area; obtaining first dynamic parameters of the work machine, the first dynamic parameters comprising first three-dimensional position data and first attitude data of the work machine during operation; calculating the key point coordinates on the work machine according to the first dynamic parameters; According to the key point coordinates, a minimum boundary distance of the key point from the three-dimensional electronic fence is calculated; According to the minimum boundary distance, a safety control measure is determined and executed.

[0006] According to the above technical means, the method sets a three-dimensional electronic fence based on an actual working environment, accurately covers a three-dimensional direction, determines key point coordinates of the working machine based on dynamic parameters of the working machine, and determines a minimum boundary distance of the key point from the three-dimensional electronic fence, so that the relative positional relationship between the key point of the working machine and the three-dimensional electronic fence can be accurately determined when the position of the working machine changes, manual recalibration confirmation is not required, dynamic position self-adaptation of the working machine is realized, misoperation or collision accidents are avoided, and construction safety is significantly improved.

[0007] In some feasible embodiments, the three-dimensional electronic fence is set based on an actual working environment of a working area, including: Second dynamic parameters of the working machine when the working machine is calibrated at each calibration point are acquired, the calibration point is a position point in the actual working environment of the working area, and the second dynamic parameters include second three-dimensional position data and second attitude data; According to the second dynamic parameters, world coordinates of the calibration point are calculated; According to the world coordinates of the calibration point, a closed three-dimensional electronic fence is formed.

[0008] According to the above technical means, the calibration point in the actual working environment is calibrated by the working machine, and then the closed three-dimensional electronic fence is formed according to the world coordinates of the calibration point. The three-dimensional electronic fence formed in this way can be fitted to the actual terrain, and for irregular areas, the three-dimensional electronic fence can better reduce the risk of misencounter.

[0009] In some feasible embodiments, according to the second dynamic parameters, the world coordinates of the calibration point are calculated, including: According to the second attitude data of the working machine and the geometric structure of the working machine, a first coordinate transformation matrix from the coordinate origin of the working machine to the contact position of the working machine and the calibration point is determined; According to the second three-dimensional position data of the working machine and the first coordinate transformation matrix, the world coordinates of the calibration point are calculated.

[0010] According to the above technical means, the first coordinate transformation matrix from the work machine coordinate origin to the contact position of the work machine and the calibration point can be determined by using the inherent parameters of the work machine, and the world coordinates of the calibration point can be obtained based on the first coordinate transformation matrix and the second three-dimensional position data of the current work machine. This method uses the combined navigation module deployed on the work machine body, can quickly calculate the world coordinates of the calibration point, and does not need the user to determine the coordinates of the three-dimensional electronic fence through an additional positioning device, thereby saving cost and fence setting time.

[0011] In some possible embodiments, the key point coordinates on the work machine are calculated according to the first dynamic parameters, including: A second coordinate transformation matrix from the work machine coordinate origin to the key point is determined according to the first attitude data of the work machine and the geometric structure of the work machine; The world coordinates of the key point are calculated according to the first three-dimensional position data of the work machine and the second coordinate transformation matrix.

[0012] According to the above technical means, the second coordinate transformation matrix from the work machine coordinate origin to the key point can be determined by using the inherent parameters of the work machine, and the world coordinates of the key point can be obtained based on the second coordinate transformation matrix and the first three-dimensional position data of the current work machine. This method uses the combined navigation module deployed on the work machine body, can quickly calculate the world coordinates of the key point, and can quickly calculate the world coordinates of the key point even if the dynamic parameters of the work machine change, without manual repeated calibration, thereby greatly improving work efficiency.

[0013] In some possible embodiments, the minimum boundary distance between the key point and the three-dimensional electronic fence is calculated according to the key point coordinates, including: According to the first attitude data, when the work machine remains stationary and the direction is fixed, the minimum boundary distance between the key point and the three-dimensional electronic fence is calculated according to the key point coordinates by using the point-to-cube distance calculation formula.

[0014] In some possible embodiments, the minimum boundary distance between the key point and the three-dimensional electronic fence is calculated according to the key point coordinates, including: According to the first attitude data, when the work machine is tilted, the minimum boundary distance between the key point and the three-dimensional electronic fence is calculated according to the key point coordinates by using the point-to-triangle distance calculation formula.

[0015] According to the above technical means, whether the work machine is tilted or not, the minimum boundary distance between the key point and the three-dimensional electronic fence is calculated in different ways, which can simplify the calculation method while ensuring the accuracy of the calculation result, and significantly improve the construction safety.

[0016] In some possible embodiments, the method further comprises: predicting a future path segment of the working machine according to the first dynamic parameter; calculating a minimum predicted boundary distance of a key point from the three-dimensional electronic fence in each predicted point according to the future path segment; determining and executing an early control measure according to the minimum predicted boundary distance and a time corresponding to the predicted point.

[0017] According to the above technical means, the future path segment of the working machine can be predicted, and early control can be performed according to the minimum predicted boundary distance, so as to avoid the safety hazard caused by insufficient emergency brake buffer distance of the working device.

[0018] In some possible embodiments, predicting the future path segment of the working machine according to the first dynamic parameter comprises: predicting future attitude data and future three-dimensional position data of the working machine according to the first dynamic parameter and a set time interval; determining a third coordinate transformation matrix from a coordinate origin of the working machine to the key point according to the future attitude data and a geometric structure of the working machine; calculating future world coordinates of the key point according to the future three-dimensional position data and the third coordinate transformation matrix, and the future world coordinates of the same key point at different times forming a future path segment corresponding to the key point.

[0019] The second aspect of the present application provides a working machine safety operation control device based on a three-dimensional electronic fence, the device comprising: a fence setting unit configured to set a three-dimensional electronic fence based on an actual terrain of a working area; a data acquisition unit configured to acquire a first dynamic parameter of a working machine, the first dynamic parameter comprising first three-dimensional position data and first attitude data of the working machine; a coordinate conversion unit configured to calculate coordinates of a key point on the working machine according to the first dynamic parameter; a distance calculation unit configured to calculate a minimum boundary distance of the key point from the three-dimensional electronic fence according to the coordinates of the key point; an execution unit configured to determine and execute a safety control measure according to the minimum boundary distance.

[0020] According to the above technical means, the device sets the three-dimensional electronic fence based on the actual working environment, accurately covers the three-dimensional direction, determines the key point coordinates of the working machine based on the dynamic parameters of the working machine, to determine the minimum boundary distance between the key point and the three-dimensional electronic fence, and accurately determine the relative position relationship between the key point of the working machine and the three-dimensional electronic fence when the position of the working machine changes, without manual re-calibration confirmation, realizing the dynamic position adaptation of the working machine, avoiding misoperation or collision accidents, and significantly improving the construction safety.

[0021] The third aspect of the application provides a machine-readable storage medium, which stores instructions for causing a machine to execute the working machine safety operation control method based on the three-dimensional electronic fence.

[0022] Through the above technical solution, a working machine safety operation control method based on a three-dimensional electronic fence is provided, which sets the three-dimensional electronic fence based on the actual working environment, accurately covers the three-dimensional direction, determines the key point coordinates of the working machine based on the dynamic parameters of the working machine, to determine the minimum boundary distance between the key point and the three-dimensional electronic fence, and accurately determine the relative position relationship between the key point of the working machine and the three-dimensional electronic fence when the position of the working machine changes, without manual re-calibration confirmation, realizing the dynamic position adaptation of the working machine, avoiding misoperation or collision accidents, and significantly improving the construction safety.

[0023] Other features and advantages of the embodiments of the application will be described in detail in the following specific embodiments. BRIEF DESCRIPTION OF DRAWINGS

[0024] The accompanying drawings are included to provide a further understanding of the embodiments of the application, and constitute a part of the specification, and are used to explain the embodiments of the application together with the following specific embodiments, but do not constitute a limitation of the embodiments of the application. In the drawings: Figure 1 A working machine system topology diagram according to an embodiment of the application is schematically shown; Figure 2 A working machine safety operation control method based on a three-dimensional electronic fence according to an embodiment of the application is schematically shown in a step diagram; Figure 3 A whole excavator modeling diagram according to an embodiment of the application is schematically shown; Figure 4 A fence diagram when the excavator moves and tilts according to an embodiment of the application is schematically shown; Figure 5 A two-dimensional fence rotation restriction area and ideal area diagram of an excavator according to an embodiment of the application is schematically shown; Figure 6Another method for controlling the safety operation of a working machine based on a three-dimensional electronic fence is shown in a schematic flowchart according to an embodiment of the present application. Figure 7 A general control flowchart according to the method of the present application is shown in a schematic block diagram. DETAILED DESCRIPTION

[0025] To make the objectives, technical solutions, and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. It should be understood that the specific implementation manners described herein are only used to illustrate and explain the embodiments of the present application and should not be used to limit the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by a person of ordinary skill in the art without creative work fall within the scope of protection of the present application.

[0026] It should be noted that the acquisition, transmission, storage, use, processing, and the like of data in the technical solutions of the present application comply with relevant provisions of laws and regulations. In the embodiments of the present application, some industry existing solutions, components, models, and the like may be mentioned, which should be considered as exemplary, and the purpose is only to illustrate the feasibility of the implementation of the technical solutions of the present application, but does not mean that the applicant has or will necessarily use the solutions.

[0027] It should be noted that if the embodiments of the present application involve directional indications (such as up, down, left, right, front, back, and the like), the directional indications are only used to explain the relative positional relationship, movement condition, and the like between components in a certain posture (as shown in the drawings), and if the certain posture changes, the directional indications also change accordingly.

[0028] In addition, if the embodiments of the present application involve descriptions of “first”, “second”, and the like, the descriptions of “first”, “second”, and the like are only for description purposes and should not be understood as indicating or implying the relative importance of the indicated technical features or implicitly indicating the number of the indicated technical features. Therefore, the features limited by “first” and “second” can explicitly or implicitly include at least one of the features. In addition, the technical solutions of the various embodiments can be combined with each other, but must be based on the fact that a person of ordinary skill in the art can implement the combination, and when the combination of the technical solutions contradicts each other or cannot be implemented, it should be considered that the combination of the technical solutions does not exist and is not within the scope of protection claimed by the present application.

[0029] The existing electronic fence has the following disadvantages: 1. Only planar boundaries can be defined, and spatial constraints are limited. Specifically, the limiting dimension is single: usually only one or more horizontal and vertical boundaries can be limited, such as height and depth limits; then it cannot form a complete spatial closed region, and cannot effectively define a three-dimensional forbidden space, but can only limit part of the direction.

[0030] 2. Weak adaptability to complex environments. Specifically, in complex spatial structure sites such as tunnels, bridges, and around different buildings, two-dimensional boundaries alone cannot accurately cover all directions, such as on inclined ground, multi-layer steep slopes, etc., and cannot conform to the actual terrain for dynamic protection.

[0031] 3. Lack of dynamic posture adaptive ability. Traditional two-dimensional fences are mostly static, and do not have the ability to dynamically adjust the boundary according to the real-time posture of the working machine. Once the position or posture of the working machine changes, manual recalibration is required, resulting in low work efficiency and difficulty in responding to the working state in a timely manner.

[0032] To solve the above technical problems, the present application proposes a working machine safety operation control method based on a three-dimensional electronic fence, which uses the navigation positioning system of the working machine itself to collect the three-dimensional position and posture of the working machine in the world coordinate system. In the working space of the working machine (such as an excavator), a fully enclosed or semi-enclosed three-dimensional electronic fence is defined using a three-dimensional coordinate system. With the help of the navigation positioning system, the working machine itself can sense the spatial position of the obstacle relative to the machine when the working machine moves or its posture changes. The system calculates the position and posture of the end effector (such as a bucket) or other key components of the working machine in real time. When it is about to touch the virtual boundary, it automatically issues a warning, and can perform speed limiting or emergency stop according to the settings and other safety control measures.

[0033] For example, Figure 1A work machine system topology diagram of the present application is shown. The system includes a main controller, an instrument, a collection unit, and an execution unit, the collection unit is used to collect dynamic parameters of the work machine, including real-time dynamic positioning module (Real-time kinematic, RTK), inertial measurement unit (inertial measurement unit, IMU), pressure sensor, and inclination sensor, etc., the execution unit is used to control the valve core current, pump displacement, and engine power, etc. The main controller determines the coordinates of the key points on the work machine in real time according to the first dynamic parameters collected by the collection unit, and then calculates the minimum boundary distance between the key points and the three-dimensional electronic fence. According to the minimum boundary distance, it is determined whether the work machine is about to touch the boundary of the three-dimensional electronic fence, and when it is about to touch the boundary of the three-dimensional electronic fence, the safety control measures are executed. The main controller includes but is not limited to: a computing unit based on a single-chip microcomputer, an industrial computer based on a CPU, a CPU+TPU processor, etc.; the real-time dynamic positioning module can be replaced by a Global Navigation Satellite System (Global Navigation Satellite System, GNSS), a precise point positioning (precise point positioning, PPP), a Satellite-Based Augmentation System (Satellite-Based Augmentation System, SBAS) positioning unit, etc. The inertial measurement unit can be replaced by a visual-inertial odometry (Visual-Inertial Odometry, VIO), Doppler radar, laser radar, binocular depth camera, etc.

[0034] Figure 2 The steps of a work machine safety operation control method based on a three-dimensional electronic fence according to an embodiment of the present application are shown. As shown in Figure 2 , the method can include the following steps.

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

[0036] In some feasible embodiments, setting a three-dimensional electronic fence based on the actual work environment of the work area includes: S101: Obtain a second dynamic parameter of the working machine when calibrating each calibration point, the calibration point being a position point in an actual working environment of the working area, and the second dynamic parameter including second three-dimensional position data and second attitude data. Taking an excavator as an example, when setting a three-dimensional electronic fence, the operator can control the excavator to contact the tooth tip with the position point in the actual working environment, and record the three-dimensional position data and the attitude data of the excavator at the time of contact as the dynamic parameter. The three-dimensional position data mainly includes the coordinates of the working machine in the world coordinate system, and the attitude data mainly includes the roll angle, the rotation angle, the pitch angle, the roll angle, etc. of the working machine. In order to facilitate the distinction, the dynamic parameter in the calibration process is recorded as the second dynamic parameter, the three-dimensional position data is recorded as the second three-dimensional position data, and the attitude data is recorded as the second attitude data.

[0037] S102: Then, the world coordinates of the calibration point are calculated according to the second dynamic parameter, specifically including: According to the second attitude data of the working machine and the geometric structure of the working machine, a first coordinate transformation matrix from the coordinate origin of the working machine to the contact position of the working machine and the calibration point is determined. In the embodiment of the application, the first coordinate transformation matrix refers to the transformation matrix from the coordinate origin of the working machine to the contact position of the working machine and the calibration point.

[0038] In the embodiment of the application, the real-time joint angle values of the excavator boom, the dipper arm and the bucket can be calculated according to the roll angle in the second attitude data. For example, Figure 3 The modeling schematic diagram of the whole excavator is shown, which shows the geometric structure modeling of the excavator, wherein the boom joint angle value is directly measured by the boom roll angle , that is, . The dipper arm joint angle value is the difference between the dipper arm roll angle and the boom roll angle , that is, . The bucket joint angle value needs to be obtained through some geometric operations, specifically, the roll angle installed on the GI connecting rod is converted into the included angle value of the HL and the horizontal line , and then the boom and dipper arm roll angles are integrated to obtain .

[0039] After obtaining the joint angle, the rotation center coordinate system Q is converted to the boom hinge point O to obtain: ; wherein, is the rotation joint angle, is the connecting rod length of the boom hinge point coordinate system O point and the rotation center coordinate system Q point, is the joint distance of the boom hinge point coordinate system O point and the rotation center coordinate system Q point, The offset distance of the connecting rod of the boom hinge point coordinate system O point and the swing center coordinate system Q point.

[0040] The transformation matrix from the boom hinge point coordinate system O to the stick hinge point coordinate system E is: ; Wherein, The connecting rod length of the boom hinge point coordinate system O point to the stick hinge point E, The joint distance of the boom hinge point coordinate system O point to the stick hinge point E.

[0041] The transformation matrix from the stick hinge point coordinate system E to the bucket hinge point coordinate system H is: ; Wherein, The connecting rod length of the stick hinge point coordinate system E point to the bucket hinge point H, The joint distance of the stick hinge point coordinate system E point to the bucket hinge point H.

[0042] The transformation matrix from the bucket hinge point coordinate system H to the bucket tooth tip coordinate system L is: ; Wherein, The connecting rod length of the bucket hinge point coordinate system H point to the bucket tooth tip L, The joint length of the bucket hinge point coordinate system H point to the bucket tooth tip L.

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

[0044] The rotation angle can be obtained by installing a rotation encoder at the connection between the swing and the lower swing of the excavator body, and the pitch angle and roll angle of the body can be obtained after calibrating the inclination sensor installed on the body. The above angles and the structure parameters of the excavator are analyzed by forward kinematics analysis, and the coordinate transformation matrix of the body relative to the earth is derived.

[0045] In some embodiments, the excavator tooth tip is calibrated by contacting the actual environment of the working area, and the transformation matrix from the swing center coordinate system Q to the bucket tooth tip coordinate system L is used as the first coordinate transformation matrix. If other parts of the excavator are calibrated by contacting the actual environment of the working area, the transformation matrix is also derived by using geometric principles combined with the whole machine modeling diagram.

[0046] The pose of the bucket tip is represented as , the expansion can obtain: ; The rotation angle can be obtained by installing a rotation encoder at the connection between the swing body and the lower swing of the excavator, and the pitch angle and roll angle of the swing body can be obtained by installing an inclination sensor on the swing body after calibration. The above angles and the structural parameters of the excavator are subjected to forward kinematics analysis to derive the coordinate transformation matrix of the swing body relative to the earth.

[0047] Then, the world coordinates of the calibration point are calculated according to the second three-dimensional position data of the working machine and the first coordinate transformation matrix. In this way, the first coordinate transformation matrix from the origin of the working machine coordinates to the position where the working machine contacts the calibration point can be determined by using the inherent parameters of the working machine, and the world coordinates of the calibration point can be obtained based on the first coordinate transformation matrix and the second three-dimensional position data of the current working machine. This method uses the combined navigation module deployed on the working machine body, can quickly calculate the world coordinates of the calibration point, and does not require the user to determine the coordinates of the three-dimensional electronic fence by using additional positioning equipment, thereby saving cost and fence setting time.

[0048] S103: Form a closed three-dimensional electronic fence according to the world coordinates of the calibration point. In some possible embodiments, when forming the closed three-dimensional electronic fence, the fence is expanded by a preset distance away from the obstacle to reserve sufficient space for avoiding the obstacle. In one possible embodiment, the preset distance is 5-10 cm.

[0049] The limiting boundary in the working conditions such as quarries, ponds, and tunnels often presents a polygonal or arc shape, and simply relying on horizontal or vertical plane boundaries cannot fit the actual terrain. In the present application, the working machine is used to calibrate the calibration point in the actual working environment, and then a closed three-dimensional electronic fence is formed according to the world coordinates of the calibration point. This way of forming a three-dimensional electronic fence can fit the actual terrain and has better adaptability. It is not limited to planar boundaries, but can also flexibly define various obstacle boundaries such as inclined surfaces, curved surfaces, and complex bodies. For irregular areas such as quarries, ponds, and tunnels, the three-dimensional electronic fence can better reduce the risk of accidental collision. Figure 5 The excavator two-dimensional fence swing limiting area and ideal area are shown. The swing limiting area forms a cuboid area with a fan-shaped bottom surface, while the ideal area is a cuboid area with a quadrilateral bottom surface.

[0050] In other possible embodiments, the three-dimensional electronic fence can also be set in a manual setting manner, that is, the center of the circular arc, the starting point of the circular arc, and the circular arc drawing circular region are determined manually, and finally the enter region is clicked to allow or prohibit entering the region to perform protection in the instrument.

[0051] S2: Obtain a first dynamic parameter of the working machine, the first dynamic parameter comprising first three-dimensional position data and first attitude data of the working machine during working. In the embodiments of the present application, the first dynamic parameter is the dynamic parameter of the working machine during working, and in order to distinguish, the dynamic parameter of the working machine during working is referred to as the first dynamic parameter in the present application, the three-dimensional position data in the first dynamic parameter is referred to as the first three-dimensional position data, and the attitude data is referred to as the first attitude data.

[0052] S3: Calculate a key point coordinate on the working machine according to the first dynamic parameter.

[0053] In some feasible embodiments, the calculation of the key point coordinate on the working machine according to the first dynamic parameter comprises: According to the first attitude data of the working machine and the geometric structure of the working machine, a second coordinate transformation matrix from the coordinate origin of the working machine to the key point is determined. In the embodiments of the present application, each key point corresponds to a coordinate transformation matrix, and the coordinate transformation matrix is determined in the same way as the first coordinate transformation matrix in the calibration process, that is, the determined joint angle value is combined with the structure parameters of the working machine to determine the forward kinematics analysis, which will not be described here.

[0054] According to the first three-dimensional position data of the working machine and the second coordinate transformation matrix, the world coordinate of the key point is calculated.

[0055] According to the above technical means, the second coordinate transformation matrix from the coordinate origin of the working machine to the key point can be determined by using the inherent parameters of the working machine, and based on the second coordinate transformation matrix and the first three-dimensional position data of the current working machine, the world coordinate of the key point can be obtained. This method uses the combined navigation module deployed on the upper part of the working machine, and can quickly calculate the world coordinate of the key point. Even if the dynamic parameter of the working machine changes, the world coordinate of the key point can also be quickly calculated without manual repeated calibration, thereby greatly improving the working efficiency.

[0056] S4: Calculate the minimum boundary distance between the key point and the three-dimensional electronic fence according to the key point coordinate. As shown in Figure 4 During working, the distance between the working machine and the three-dimensional electronic fence will change after translation or inclination of the working machine. According to the key point coordinate, the minimum distance between the working machine and the three-dimensional electronic fence can be calculated to determine that the working machine will not collide with the obstacle, etc., so as to take safety control measures.

[0057] In some feasible embodiments, the calculation of the minimum boundary distance between the key point and the three-dimensional electronic fence according to the key point coordinate comprises: According to the first attitude data, when it is determined that the working machine remains stationary and direction fixed, a point-to-cube distance calculation formula is used to calculate the minimum boundary distance between the key point and the three-dimensional electronic fence according to the key point coordinates. ; wherein, is the key point coordinates, is a point on the cube, , are the minimum and maximum values of the cube on the xyz axis respectively, and in practice, is a point on the three-dimensional electronic fence. Assuming that the key point coordinates are (4, 6, 2) and the cube is a cube with a side length of 3, the coordinates of the six vertices of the cube are (3, 0, 0), (0, 3, 0), (0, 0, 3), (3, 3, 0), (3, 0, 3), (0, 3, 3), (3, 3, 3), and (0, 0, 0). According to these coordinate points, it can be determined that the minimum value of the cube on the y-axis is 0, the maximum value is 3, the minimum value of the cube on the x-axis is 0, the maximum value is 3, the minimum value of the cube on the z-axis is 0, and the maximum value is 3. Using the above formula, it can be obtained that , , , ,

[0058] In other feasible embodiments, the minimum boundary distance between the key point and the three-dimensional electronic fence is calculated according to the key point coordinates, including: According to the first attitude data, when it is determined that the working machine is tilted, a point-to-triangle distance calculation formula is used to calculate the minimum boundary distance between the key point and the three-dimensional electronic fence according to the key point coordinates.

[0059] According to the above technical means, according to whether the working machine is tilted, the minimum boundary distance between the key point and the three-dimensional electronic fence is calculated in different ways, which can simplify the calculation method while ensuring the accuracy of the calculation result, and significantly improve the construction safety.

[0060] S5: Determine and execute safety control measures according to the minimum boundary distance. The 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 the first preset distance and greater than or equal to the second preset distance, a first-level warning is issued and speed limiting is performed; when the minimum boundary distance is less than the second preset distance, a second-level warning is issued and economic stopping is performed. ​​​

[0061] According to the above technical means, the method sets a three-dimensional electronic fence based on an actual working environment, accurately covers a three-dimensional direction, determines a key point coordinate of the working machine based on a dynamic parameter of the working machine, and determines a minimum boundary distance between the key point and the three-dimensional electronic fence, so that the relative positional relationship between the key point of the working machine and the three-dimensional electronic fence can be accurately determined when the position of the working machine changes, manual re-calibration confirmation is not required, dynamic position self-adaptation of the working machine is realized, misoperation or collision accidents are avoided, and construction safety is significantly improved.

[0062] In some feasible embodiments, as shown in Figure 6 The method further includes: S6: predicting a future path segment of the working machine according to the first dynamic parameter. Specifically, the method includes: predicting future attitude data and future three-dimensional position data of the working machine according to the first dynamic parameter and a set time interval.

[0063] In the embodiments of the present application, the current joint angle (boom, stick, bucket) and the angular velocity thereof are used to predict future actions. The state space equation is simplified as: ; wherein, is a time interval, is an angle at time t, denotes an angular velocity.

[0064] determining a third coordinate transformation matrix from a coordinate origin of the working machine to the key point according to the future attitude data and the geometric structure of the working machine; calculating future world coordinates of the key point according to the future three-dimensional position data and the third coordinate transformation matrix, and the future world coordinates of the same key point at different times forming a future path segment corresponding to the key point.

[0065] According to the whole machine kinematics, the position of the key point in the world coordinates at the future time is calculated , so as to predict the position of the tooth tip or the key component, and continuously predict n future time points to form a path segment: .

[0066] S7: calculating a minimum predicted boundary distance between the key point and the three-dimensional electronic fence in each prediction point according to the future path segment, and the minimum predicted boundary distance is calculated in the same way as the minimum boundary distance.

[0067] S8: determining and executing an early control measure according to the minimum predicted boundary distance and the time corresponding to the prediction point.

[0068] The early control measure can be to perform speed limiting at a time point in advance by a certain time length on the basis of the time point corresponding to the prediction point when it is predicted that the minimum prediction boundary distance reaches the first warning and speed limiting condition at a future time point, for example, the current time point is 15:00:00, it is predicted that the minimum prediction boundary distance reaches the first warning and speed limiting condition at 15:01:10, and then the warning and speed limiting are performed at 15:01:05. Other safety control measures are sequentially set.

[0069] According to the above technical means, the future path segment of the working machine can be predicted, and the control is performed in advance according to the minimum prediction boundary distance, so as to avoid the safety hidden danger caused by the insufficient emergency brake buffer distance of the working device.

[0070] As Figure 7 The overall control flowchart of the method of the application is shown, the method of the application establishes the three-dimensional electronic fence boundary in a calibration manner, in the working process of the working machine, the second dynamic parameters are collected by the multiple sensors and input into the main controller, the main controller performs pose / position fusion, and the spatial positions of each moving part on the working machine are calculated. On the one hand, the nearest distance is judged by using the spatial positions of each moving part on the working machine and the three-dimensional boundary of the three-dimensional electronic fence, real-time decision output is generated, corresponding control commands are generated, and the hydraulic actuator is controlled to execute; on the other hand, the future path segment is predicted based on the calculated spatial positions, whether the future path segment will exceed the three-dimensional electronic fence boundary is predicted, the decision output is generated based on the out-of-boundary prediction result, corresponding control commands are generated, and the hydraulic actuator is controlled to execute.

[0071] The second aspect of the application provides a working machine safety working control device based on a three-dimensional electronic fence, the device comprises: The fence setting unit is used for setting the three-dimensional electronic fence based on the actual terrain of the working area; The data acquisition unit is used for acquiring the first dynamic parameters of the working machine, the first dynamic parameters comprising the first three-dimensional position data and the first attitude data of the working machine; The coordinate conversion unit is used for calculating the key point coordinates on the working machine according to the first dynamic parameters; The distance calculation unit is used for calculating the minimum boundary distance between the key points and the three-dimensional electronic fence according to the key point coordinates; The execution unit is used for determining and executing the safety control measures according to the minimum boundary distance.

[0072] According to the above technical means, the device sets the three-dimensional electronic fence based on the actual working environment, accurately covers the three-dimensional direction, determines the key point coordinates of the working machine based on the dynamic parameters of the working machine, to determine the minimum boundary distance between the key point and the three-dimensional electronic fence, and accurately determine the relative position relationship between the key point of the working machine and the three-dimensional electronic fence when the position of the working machine changes, without manual re-calibration confirmation, realizing the dynamic position adaptation of the working machine, avoiding misoperation or collision accidents, and significantly improving the construction safety.

[0073] The third aspect of the present application provides a machine-readable storage medium, which stores instructions for causing a machine to execute the working machine safety operation control method based on the three-dimensional electronic fence.

[0074] Through the above technical solution, a working machine safety operation control method based on a three-dimensional electronic fence is provided, which sets the three-dimensional electronic fence based on the actual working environment, accurately covers the three-dimensional direction, determines the key point coordinates of the working machine based on the dynamic parameters of the working machine, to determine the minimum boundary distance between the key point and the three-dimensional electronic fence, and accurately determine the relative position relationship between the key point of the working machine and the three-dimensional electronic fence when the position of the working machine changes, without manual re-calibration confirmation, realizing the dynamic position adaptation of the working machine, avoiding misoperation or collision accidents, and significantly improving the construction safety.

[0075] Those skilled in the art will understand that the embodiments of the present application can be provided as a method, system or computer program product. Therefore, the present application can take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can take the form of a computer program product implemented 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] The present application is described with reference to flowcharts and / or block diagrams of methods, devices (systems), computer program products according to embodiments of the present application. It should be understood that each flow and / or block in the flowcharts and / or block diagrams can be implemented by computer program instructions. These computer program instructions can be provided to a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing apparatus to produce a machine, so that the instructions executed by the computer or other programmable data processing apparatus produce a device that implements the functions specified in the flowcharts and / or block diagrams. Figure 1 The functions specified in one or more flows and / or blocks. Figure 1 The device that implements the functions specified in one or more flows and / or blocks.

[0077] These computer program instructions can also be stored in a computer- readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instructions which implement the Figure 1 function specified in the flow or flows and / or blocks Figure 1 of the block or blocks.

[0078] These computer program instructions can 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 such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the Figure 1 function specified in the flow or flows and / or blocks Figure 1 of the block or blocks.

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

[0080] The memory can include non-persistent memory and / or volatile memory, such as a random access memory (RAM) including a cache area for the temporary storage of data. The memory can also include non-volatile memory, such as read only memory (ROM), electrically programmable read only memory (EPROM), electrically erasable programmable read only memory (EEPROM), flash memory, or non-volatile random access memory (NVRAM), for the storage of information, such as data files or program

[0081] Computer readable media includes permanent and non-permanent, removable and non-removable media implemented in any method or technology for the storage of information. The information can be computer readable instructions, data structures, program modules, 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 technology, compact disc read only memory (CD-ROM), digital versatile discs (DVDs) or other optical storage, magnetic cassettes, magnetic disks storage or other magnetic storage devices, or any other non-transmission medium that can be used to store information accessible to a computing device. According to the definition herein, computer readable media does not include transitory media, such as modulated data signals and carrier waves.

[0082] It should also be noted that the terms "comprising", "comprises" or other variations thereof are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but can also include other elements not expressly listed or inherent to such process, method, article, or apparatus.

[0083] The above embodiments are only used to illustrate the present application, but not to limit it. Instead of the above, various modifications and changes can be made to the application by those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the application shall fall into the scope of the claims of the application.

Claims

1. A method for safe work control of a work machine based on a three-dimensional electronic fence, characterized by, The method comprises: Setting a three-dimensional electronic fence based on the actual working environment of the working area; Obtaining a first dynamic parameter of the working machine, the first dynamic parameter comprising first three-dimensional position data and first attitude data of the working machine when working; Calculating a key point coordinate on the working machine according to the first dynamic parameter; Calculating a minimum boundary distance between the key point and the three-dimensional electronic fence according to the key point coordinate; Determining and executing a safety control measure according to the minimum boundary distance.

2. The stereoelectronical fence based work machine safe work control method according to claim 1, characterized by, Setting a three-dimensional electronic fence based on the actual working environment of the working area comprises: Obtaining a second dynamic parameter of the working machine when calibrating each calibration point, the calibration point being a position point in the actual working environment of the working area, the second dynamic parameter comprising second three-dimensional position data and second attitude data; Calculating a world coordinate of the calibration point according to the second dynamic parameter; Forming a closed three-dimensional electronic fence according to the world coordinate of the calibration point.

3. The stereoelectronical fence based work machine safe work control method according to claim 2, characterized by, Calculating a world coordinate of the calibration point according to the second dynamic parameter comprises: Determining a first coordinate transformation matrix from the coordinate origin of the working machine to the contact position of the working machine and the calibration point according to the second attitude data of the working machine and the geometric structure of the working machine; Calculating the world coordinate of the calibration point according to the second three-dimensional position data of the working machine and the first coordinate transformation matrix.

4. The stereoelectronical fence based work machine safe work control method according to claim 1, characterized by, Calculating a key point coordinate on the working machine according to the first dynamic parameter comprises: Determining a second coordinate transformation matrix from the coordinate origin of the working machine to the key point according to the first attitude data of the working machine and the geometric structure of the working machine; Calculating a world coordinate of the key point according to the first three-dimensional position data of the working machine and the second coordinate transformation matrix.

5. The stereoelectronical fence based work machine safe work control method according to claim 1, characterized by, Calculating a minimum boundary distance between the key point and the three-dimensional electronic fence according to the key point coordinate comprises: When the working machine remains stationary and the direction is fixed, a point-to-cube distance calculation formula is used to calculate the minimum boundary distance between the key point and the three-dimensional electronic fence according to the key point coordinate, according to the first attitude data.

6. The stereoelectronical fence based work machine safe work control method according to claim 1, characterized by, Calculating a minimum boundary distance between the key point and the three-dimensional electronic fence according to the key point coordinate comprises: When the working machine is tilted, a point-to-triangle distance calculation formula is used to calculate the minimum boundary distance between the key point and the three-dimensional electronic fence according to the key point coordinate, according to the first attitude data.

7. The stereoelectronical fence based work machine safe work control method according to claim 1, characterized by, The method further comprises: Predicting a future path segment of the working machine according to the first dynamic parameter; Calculating a minimum predicted boundary distance between the key point and the three-dimensional electronic fence in each predicted point according to the future path segment; Determining and executing an advance control measure according to the minimum predicted boundary distance and the time corresponding to the predicted point.

8. The stereoelectronical fence based work machine safe work control method according to claim 7, characterized by, Predicting a future path segment of the working machine according to the first dynamic parameter comprises: Predicting future attitude data and future three-dimensional position data of the working machine according to the first dynamic parameter and a set time interval; Determining a third coordinate transformation matrix from the coordinate origin of the working machine to the key point according to the future attitude data and the geometric structure of the working machine; According to the future three-dimensional position data and the third coordinate transformation matrix, future world coordinates of key points are calculated, and future path segments corresponding to the key points are formed by the future world coordinates of the same key points at different times.

9. A work machine safety work control device based on a stereoscopic electronic fence, characterized by, The device comprises: a fence setting unit configured to set a three-dimensional electronic fence based on an actual terrain of a work area; a data acquisition unit configured to acquire first dynamic parameters of a work machine, the first dynamic parameters including first three-dimensional position data and first attitude data of the work machine; a coordinate conversion unit configured to calculate key point coordinates on the work machine according to the first dynamic parameters; a distance calculation unit configured to calculate minimum boundary distances between the key points and the three-dimensional electronic fence according to the key point coordinates; an execution unit configured to determine and execute safety control measures according to the minimum boundary distances.

10. A machine-readable storage medium, characterized in that, The machine-readable storage medium has instructions stored thereon for causing a machine to perform the safety work control method of the work machine based on the three-dimensional electronic fence according to any one of claims 1 to 8.

Images