CN117864966A

CN117864966A - Cooperative control method, cooperative control system and engineering machinery group

Info

Publication number: CN117864966A
Application number: CN202211236865.9A
Authority: CN
Inventors: 余闯; 于晓颖; 付玲; 林能发; 陈耀琦; 曾维国
Original assignee: Zoomlion Heavy Industry Science and Technology Co Ltd
Current assignee: Zoomlion Heavy Industry Science and Technology Co Ltd
Priority date: 2022-10-10
Filing date: 2022-10-10
Publication date: 2024-04-12

Abstract

The invention relates to the field of engineering machinery, and discloses a cooperative control method, a cooperative control system and an engineering machinery group. The cooperative control method comprises the following steps: predicting a relative motion trend between a first joint of a first device and a second joint of a second device; determining a control strategy for the first joint and the second joint according to respective speeds of the first joint and the second joint and a joint relative distance between the first joint and the second joint when the relative movement trend indicates that the two joints have a approaching trend; and controlling the first joint to execute a first sub-control strategy and the second joint to execute a second sub-control strategy according to a construction method avoiding principle so as to prevent the first equipment from colliding with the second equipment. The invention can automatically control the equipment to cooperatively work, can ensure that the engineering machinery with larger motion inertia can timely detect collision risk, and adopts a safety avoidance measure.

Description

Cooperative control method, cooperative control system and engineering machinery group

Technical Field

The invention relates to the field of engineering machinery, in particular to a cooperative control method, a cooperative control system and an engineering machinery group.

Background

Along with the increasing of large-scale construction projects, various construction equipment coexist, high-density construction environments and high-efficiency construction rhythms become normal. The construction of multiple machines simultaneously, space-time conflict and high collision risk, once a safety accident occurs, huge losses such as qualification reduction, engineering delay and the like are caused for equipment contractors and construction undertakers.

When space-time conflict occurs, the coordinator and the operator communicate with each other to complete the avoidance action, so that the problems of untimely communication, communication understanding deviation, incapability of uniform resource scheduling by the dispatcher and the like exist, and the problems of long communication time consumption, long equipment waiting time and low construction efficiency are caused.

The existing tower crane group anti-collision strategy aims at controlling the tower crane by a person, and aims at the situation that collision is possible, equipment is simply controlled to stop moving, and finally, the equipment is required to be coordinated by the person to avoid. The method does not analyze measures for solving the collision risk for equipment, and needs to be coordinated and judged by a crane driver according to specific conditions, so that the method cannot be suitable for a cluster of automatic operation.

Disclosure of Invention

The invention aims to provide a cooperative control method, a cooperative control system and an engineering machinery group, which are based on equipment joint movement trend prediction, dynamic safety threshold calculation of movement states of all joints of equipment and a principle and a method for avoiding a site construction method, automatically control equipment to cooperatively operate, and can ensure that engineering machinery equipment with larger movement inertia can timely detect collision risks and take safety avoidance measures.

In order to achieve the above object, a first aspect of the present invention provides a cooperative control method, including: predicting the relative movement trend between a first joint of a first device and a second joint of a second device according to the movement pose of the first joint and the second joint; determining a control strategy for the first joint and the second joint according to the speed of the first joint and the speed of the second joint and the joint relative distance between the first joint and the second joint under the condition that the relative movement trend shows that the first joint and the second joint have an approaching trend, wherein the control strategy comprises a deceleration strategy, an avoidance strategy, an scram strategy and/or a normal operation strategy; and according to a construction method avoiding principle, controlling the first joint to execute a first sub-control strategy in the control strategy and the second joint to execute a second sub-control strategy in the control strategy so as to prevent the first equipment from colliding with the second equipment.

Preferably, the cooperative control method further includes: determining a horizontal operation area of the first equipment and a horizontal operation area of the second equipment; determining a limited joint collision area shared by the first equipment and the second equipment and a critical collision risk area of each of the first equipment and the second equipment under the condition that the horizontal operation area of the first equipment and the horizontal operation area of the second equipment overlap; and performing the step of predicting a relative movement trend between the first joint and the second joint in a case where the swing joint of the first device and the swing joint of the second device enter respective critical collision risk areas, respectively, wherein the defined joint collision area is a quadrangular area formed by a swing center of the first device, a first critical collision point, a swing center of the second device, and a second critical collision point; the critical collision risk area of the first device is an angle area through which the rotary joint of the first device rotates when the rotary joint of the first device stably stops at the current speed and just enters the limited joint collision area; and the critical collision risk area of the second device is an angle area through which the swing joint of the second device rotates when the swing joint of the second device is stopped steadily at the current speed and just enters the limited joint collision area, wherein in the case that the first joint and the second joint are both swing joints, the predicting the relative motion trend between the first joint and the second joint includes: and determining that the first joint and the second joint have a tendency to approach in the horizontal direction when the swing joint of the first device and the swing joint of the second device meet a horizontal approach condition. Preferably, the horizontal approach condition comprises at least one of: the revolute joint of the first device and the revolute joint of the second device enter a defined joint collision region; the rotary joint of the first device and the rotary joint of the second device enter respective critical collision risk areas simultaneously; the revolute joint of the first device entering a defined joint collision region of the first device and the revolute joint of the second device entering a critical collision risk region of the second device; the revolute joint of the second device entering a defined joint collision region of the second device and the revolute joint of the first device entering a critical collision risk region of the first device; the swivel joint of the first device and the swivel joint of the second device swivel in the same direction in a plurality of consecutive cycles, respectively.

Preferably, the first critical collision point and the second critical collision point are intersections of a contour of a horizontal operation area of the first device and a contour of a horizontal operation area of the second device.

Preferably, in a case where the amplitude-changing joint end of the first device is higher than the upper side of the revolute joint of the second device, the first joint is an amplitude-changing joint, and the second joint is a revolute joint, the predicting the relative movement trend between the first joint and the second joint includes: determining a vertical working area of the first equipment and a vertical working area of the second equipment; and determining that there is a tendency for the first joint to approach the second joint in a vertical direction when a vertical working area of the first device and a vertical working area of the second device satisfy a first vertical approaching condition, wherein the vertical working area of the first device is a rectangular area that is long by a horizontal projection length of a luffing joint of the first device and is wide by a vertical distance from a luffing joint end of the first device to a lower side of the luffing joint of the second device; and the vertical working area of the second equipment is a rectangular area with the horizontal projection length of the rotary joint of the second equipment being long and the vertical distance from the tail end of the rotary joint of the second equipment to the tail end of the lifting joint of the second equipment being wide.

Preferably, the first vertical approaching condition includes: the horizontal distance between the vertical working area of the first device and the vertical working area of the second device decreases over a plurality of consecutive cycles.

Preferably, in a case where the distal end of the luffing joint of the first apparatus is lower than the lower side of the revolute joint of the second apparatus, the first joint is a luffing joint, and the second joint is a luffing joint or a lifting joint, the predicting the relative movement trend between the first joint and the second joint includes: determining a vertical working area of the first equipment and a vertical working area of the second equipment; and determining that there is a tendency for the first joint to approach the second joint in a vertical direction when a vertical working area of the first device and a vertical working area of the second device satisfy a second vertical approaching condition, wherein the vertical working area of the first device is a rectangular area that is long by a horizontal projection length of a luffing joint of the first device and is wide by a vertical distance from a luffing joint end of the first device to a lifting joint end of the first device; and the vertical working area of the second equipment is a rectangular area with the horizontal distance from the rotation center of the second equipment to the outer side of the amplitude variation joint being long and the vertical distance from the amplitude variation joint of the second equipment to the tail end of the lifting joint of the second equipment being wide.

Preferably, the second vertical approach condition includes: the vertical working area of the first apparatus and the vertical working area of the second apparatus have overlap in a vertical direction but do not overlap in a horizontal direction, and a horizontal distance between the vertical working area of the first apparatus and the vertical working area of the second apparatus decreases in a plurality of consecutive periods; or the vertical working area of the first apparatus and the vertical working area of the second apparatus overlap in the horizontal direction but do not overlap in the vertical direction, and the vertical distance between the vertical working area of the first apparatus and the vertical working area of the second apparatus decreases in a plurality of consecutive periods.

Preferably, the determining a control strategy for the first joint and the second joint comprises: determining a control time threshold according to the speed of the first joint, the speed of the second joint, the total inertia parameters of the first equipment and the second equipment and a first corresponding relation, wherein the first corresponding relation is a corresponding relation among the first joint speed, the second joint speed, the control time and the inertia parameters; and determining the control strategy according to the control opportunity threshold and the joint relative distance between the first joint and the second joint.

Preferably, in the case where the control opportunity threshold includes a first threshold, a second threshold, and a third threshold, the determining the control strategy includes: determining a deceleration strategy and an avoidance strategy for one of the first joint and the second joint as the first sub-control strategy and a normal operation strategy for the other joint as the second sub-control strategy, in the case that a joint relative distance between the first joint and the second joint is less than the first threshold and greater than or equal to the second threshold; determining a deceleration strategy and an avoidance strategy for one of the first joint and the second joint as the first sub-control strategy and a deceleration strategy for the other joint as the second sub-control strategy, if a joint relative distance between the first joint and the second joint is less than the second threshold and greater than or equal to the third threshold; or if the joint relative distance between the first joint and the second joint is smaller than the third threshold, determining a scram strategy and a dodge strategy for one joint of the first joint and the second joint as the first sub-control strategy, and determining a scram strategy for the other joint as the second sub-control strategy, wherein the first threshold is larger than the second threshold, and the second threshold is larger than the third threshold.

Preferably, the first threshold is a joint relative distance at which the first joint and the second joint run for a first preset time at a speed threshold without collision, the second threshold is a joint relative distance at which the first joint and the second joint run for a second preset time at the speed threshold without collision, wherein the second preset time is smaller than the first preset time, and the third threshold is a joint relative distance at which the first joint and the second joint come to rest at respective current speeds without collision, wherein the speed threshold is smaller than a smaller value of the speeds of the first joint and the second joint.

Preferably, the controlling the first joint to execute a first sub-control strategy of the control strategies and the second joint to execute a second sub-control strategy of the control strategies includes: according to the construction method avoiding principle, controlling the first joint of the first equipment to execute the first sub-control strategy and the second joint of the second equipment to execute the second sub-control strategy, wherein the first equipment is no-load equipment and the second equipment is heavy-load equipment; the first device is a low-speed device and the second device is a high-speed device; or the first device is a lifting device and the second device is a pumping device.

Preferably, in the case where the swing joint of the first device and the swing joint of the second device respectively enter respective critical collision risk areas, the cooperative control method further includes: calculating a distance between any one of the plurality of feature points of the bounding box corresponding to any one joint of the first device and any one of the plurality of feature points of the bounding box corresponding to any one joint of the second device based on the center coordinates of the chassis of the first device, the center coordinates of the chassis of the second device, the coordinates of the plurality of feature points of the bounding box corresponding to each joint of the first device, and the coordinates of the plurality of feature points of the bounding box corresponding to each joint of the second device, when the coordinates of the bounding box corresponding to the swivel joint of the first device and the coordinates of the bounding box corresponding to the swivel joint of the second device indicate that there is an overlap between the horizontal operation area of the first device and the horizontal operation area of the second device; determining the approaching speed of the first joint and the second joint according to the minimum distance when the distance between the nearest two feature points on the bounding box corresponding to the first joint of the first device and the bounding box corresponding to the second joint of the second device is minimum and the minimum distance is smaller than a preset distance, wherein the approaching speed is the change rate of the minimum distance with time; and under the condition that the approach speed is smaller than 0, controlling the first joint and the second joint to execute an emergency stop strategy.

Preferably, in the case that the end of the luffing joint of the first device is higher than the upper side of the swing joint of the second device, the horizontal operation area of the first device is a circular area with the center of rotation of the first device as a center and the horizontal projection length of the luffing joint as a radius, and the horizontal operation area of the second device is a circular area with the center of rotation of the second device as a center and the horizontal distance from the center of rotation to the end of the arm support as a radius; or when the tail end of the luffing joint of the first equipment is lower than the lower side of the turning joint of the second equipment, the horizontal operation area of the first equipment is a circular area taking the turning center of the first equipment as the center of circle and taking the horizontal projection length of the arm support as the radius, and the horizontal operation area of the second equipment is a circular area taking the turning center of the second equipment as the center of circle and taking the horizontal distance from the turning center to the outer side of the luffing joint as the radius.

Through the technical scheme, the relative motion trend between the first joint of the first equipment and the second joint of the second equipment is creatively predicted; then, under the condition that the relative movement trend shows that the first joint and the second joint have approaching trend, determining a control strategy for the first joint and the second joint according to the speed of the first joint and the speed of the second joint and the joint relative distance between the first joint and the second joint; and finally, according to a construction method avoiding principle, controlling the first joint to execute a first sub-control strategy in the control strategy and the second joint to execute a second sub-control strategy in the control strategy so as to prevent the first equipment from colliding with the second equipment. Therefore, the method and the device are based on the prediction of the movement trend of the joints of the equipment, the calculation of the dynamic safety threshold value of the movement state of each joint of the equipment and the avoidance principle and method of the site construction method, and the automatic control equipment cooperatively works, so that the engineering mechanical equipment with larger movement inertia can be ensured to timely detect collision risk, and safety avoidance measures are adopted.

A second aspect of the present invention provides a cooperative control system including: trend predicting means for predicting a relative movement trend between a first joint of a first device and a second joint of a second device; the strategy determining device is used for determining a control strategy for the first joint and the second joint according to the speed of the first joint and the speed of the second joint and the joint relative distance between the first joint and the second joint when the relative movement trend shows that the first joint and the second joint have a approaching trend, wherein the control strategy comprises a deceleration strategy, an avoidance strategy, an emergency stop strategy and/or a normal operation strategy; and the control device is used for controlling the first joint to execute a first sub-control strategy in the control strategies and the second joint to execute a second sub-control strategy in the control strategies according to the construction method avoiding principle so as to prevent the first equipment from colliding with the second equipment.

Specific details and benefits of the cooperative control system provided in the present invention can be found in the above description of the cooperative control method, and are not repeated here.

A third aspect of the present invention provides a group of construction machines, the group comprising: a first device; a second device; and the cooperative control system is used for controlling the operation of the first equipment and the second equipment according to the cooperative control method.

Additional features and advantages of the invention will be set forth in the detailed description which follows.

Drawings

The accompanying drawings are included to provide a further understanding of the invention, and are incorporated in and constitute a part of this specification, illustrate the invention and together with the description serve to explain, without limitation, the invention. In the drawings:

FIG. 1 is a flow chart of a cooperative control method according to an embodiment of the present invention;

FIG. 2 is a schematic diagram illustrating a horizontal operation region division according to an embodiment of the present invention;

FIG. 3 is a schematic diagram of a vertical working area according to an embodiment of the present invention;

FIGS. 4A and 4B are schematic diagrams illustrating a vertical operation area according to an embodiment of the invention;

FIG. 5 is a flow chart of predicting motion trend based on joint spatial distance calculation according to an embodiment of the present invention;

FIG. 6 is a schematic diagram of each bounding box corresponding to a tower crane according to an embodiment of the present invention;

FIG. 7 is a schematic diagram of each bounding box corresponding to a crane according to an embodiment of the present invention;

FIG. 8 is a schematic diagram of respective bounding boxes corresponding to a pump truck according to an embodiment of the present invention;

FIG. 9 is a schematic diagram of feature points of a cube and cylindrical bounding box provided by an embodiment of the present invention; and

fig. 10 is a schematic diagram of a cooperative control platform according to an embodiment of the present invention.

Detailed Description

The following describes specific embodiments of the present invention in detail with reference to the drawings. It should be understood that the detailed description and specific examples, while indicating and illustrating the invention, are not intended to limit the invention.

With the development of the equipment to unmanned operation, the equipment gradually realizes task-based path planning and automatic operation, and if collision trend exists between the equipment, even if a certain distance exists, collision generally occurs under the condition that no control measures are taken. Therefore, in the dynamic operation process of the equipment, the relative movement trend among the equipment is more required to be detected rapidly and in real time, the collision detection is predicted in advance, and avoidance measures are adopted.

Fig. 1 is a flowchart of a cooperative control method according to an embodiment of the present invention. As shown in fig. 1, the cooperative control method may include the following steps S101 to S103.

Step S101, predicting the relative movement trend between a first joint of a first device and a second joint of a second device according to the movement pose of the first joint and the second joint;

step S102, determining a control time threshold value and a control strategy for the first joint and the second joint according to the speed of the first joint and the speed of the second joint and the joint relative distance between the first joint and the second joint when the relative movement trend indicates that the first joint and the second joint have a approaching trend, wherein the control strategy comprises a deceleration strategy, an avoidance strategy, an emergency stop strategy and/or a normal operation strategy;

step S103, according to a construction method avoiding principle, controlling the first joint to execute a first sub-control strategy in the control strategy and the second joint to execute a second sub-control strategy in the control strategy so as to prevent the first equipment from colliding with the second equipment.

The invention creatively predicts the relative motion trend between the first joint of the first equipment and the second joint of the second equipment; then, under the condition that the relative movement trend shows that the first joint and the second joint have approaching trend, determining a control strategy for the first joint and the second joint according to the speed of the first joint and the speed of the second joint and the joint relative distance between the first joint and the second joint; and finally, according to a construction method avoiding principle, controlling the first joint to execute a first sub-control strategy in the control strategy and the second joint to execute a second sub-control strategy in the control strategy so as to prevent the first equipment from colliding with the second equipment. Therefore, the method and the device are based on the prediction of the movement trend of the joints of the equipment, the calculation of the dynamic safety threshold value of the movement state of each joint of the equipment and the avoidance principle and method of the site construction method, and the automatic control equipment cooperatively works, so that the engineering mechanical equipment with larger movement inertia can be ensured to timely detect collision risk, and safety avoidance measures are adopted.

The cooperative control method may further include: determining a horizontal operation area of the first equipment and a horizontal operation area of the second equipment; determining a limited joint collision area shared by the first equipment and the second equipment and a critical collision risk area of each of the first equipment and the second equipment under the condition that the horizontal operation area of the first equipment and the horizontal operation area of the second equipment overlap; and performing the step of predicting a relative movement tendency between the first joint and the second joint in a case where the swing joint of the first device and the swing joint of the second device enter respective critical collision risk areas, respectively (i.e., step S101).

Wherein the limited joint collision area is a quadrilateral area formed by a rotation center of the first device, a first critical collision point, a rotation center of the second device and a second critical collision point; the critical collision risk area of the first device is an angle area through which the rotary joint of the first device rotates when the rotary joint of the first device stably stops at the current speed and just enters the limited joint collision area; and the critical collision risk area of the second device is an angle area through which the swing joint of the second device rotates when the swing joint of the second device stably stops at the current speed and just enters the limited joint collision area.

Before the introduction of step S101, a description is given of device space state classification based on the dynamic shared space.

First, a specific case of a horizontal work area of the apparatus will be described.

According to the relative position relation between the equipment bases and the operation range of each joint, dividing the horizontal operation area of the equipment into an area without collision risk, an area with critical collision risk and an area with limited joint collision, wherein each area is not fixed and is an area which dynamically changes along with the adjustment of the joint posture of the equipment.

When the tail end of the amplitude-changing joint of the first equipment is higher than the upper side of the rotary joint of the second equipment, the horizontal operation area of the first equipment is a circular area taking the rotary center of the first equipment as a circle center and taking the horizontal projection length of the amplitude-changing joint as a radius, and the horizontal operation area of the second equipment is a circular area taking the rotary center of the second equipment as a circle center and taking the horizontal distance from the rotary center to the tail end of the arm support as a radius; or when the tail end of the luffing joint of the first equipment is lower than the lower side of the turning joint of the second equipment, the horizontal operation area of the first equipment is a circular area taking the turning center of the first equipment as the center of circle and taking the horizontal projection length of the arm support as the radius, and the horizontal operation area of the second equipment is a circular area taking the turning center of the second equipment as the center of circle and taking the horizontal distance from the turning center to the outer side of the luffing joint as the radius.

The following description will be given by taking the division of the horizontal operation area of the crane and the tower crane as an example, as shown in fig. 2.

If the first device is a crane and the second device is a tower crane, the horizontal operation area (circle with the rotation center as the center of circle and the length of the arm support projected on the horizontal plane as the radius) of the crane will dynamically change along with the change of the luffing angle. The horizontal operation area of the tower crane (a circle taking the rotation center as the center of a circle, and the length of the horizontal arm support or the length of the arm from the rotation center to the amplitude-changing trolley as the radius) is related to the height of the horizontal arm support of the tower crane relative to surrounding equipment: when the arm support of the tower crane is higher than surrounding equipment, the radius of the horizontal operation area is the arm length from the rotation center to the luffing trolley, and the rest is the length of the horizontal arm support.

Under the condition that the boom height of the tower crane is lower than that of the crane, as shown in fig. 2, a circle corresponding to the crane is a horizontal operation area of the crane; and the larger circle corresponding to the tower crane is a horizontal operation area of the tower crane. The quadrangle formed by the points O1, A, O, B is a defined joint collision area of the crane and the tower crane. When the rotary joint of the crane is stably stopped at a first preset speed and just enters the limited joint collision area, an angle area (a sector O1CA and a sector O1C' B) of the rotary joint of the crane is a critical collision risk area of the crane; and when the rotary joint of the tower crane is stably stopped at a second preset speed and just enters the limited joint collision area, an angle area (a sector O2DA and a sector O2D' B) of the rotary joint of the tower crane, through which the rotary joint rotates, is a critical collision risk area of the tower crane. Accordingly, other areas in the horizontal work area are collision risk free areas.

Next, a specific case of the vertical operation area of the apparatus will be described.

An example of a vertical working area of the crane and the tower crane is described as shown in fig. 3 and 4.

The vertical working area of the crane is a rectangular area corresponding to the crane shown in fig. 3 or 4A (or 4B). Wherein, the horizontal projection of the arm support is long; the vertical distance from the boom end to the boom bottom end of the crane (the boom of the crane is shorter than the boom of the crane as shown in fig. 3), or the boom end to the hook end (the boom of the crane is taller than the boom end of the crane as shown in fig. 4A or 4B) is wide.

The vertical operation area of the tower crane is a rectangular area corresponding to the tower crane shown in fig. 3 or 4. The horizontal distance from the rotation center to the tail end of the arm support (namely the horizontal projection length of the arm support, the arm support of the tower crane is shorter than the arm support of the crane as shown in fig. 3), or the horizontal distance from the rotation center to the outer side of the luffing trolley (the arm support of the tower crane is higher than the tail end of the arm support of the crane as shown in fig. 4A or 4B) is longer; the vertical distance from the upper side of the arm support to the tail end of the lifting hook is wide.

Step S101, predicting the relative movement trend between a first joint of a first device and a second joint of a second device according to the movement pose of the first joint and the second joint.

The following describes the relevant content of motion trend prediction based on joint state. The motion trend prediction of the joint state is performed based on actual sensing data, and the approach trend between different joints can be judged from the horizontal aspect and the vertical aspect.

First, joint sensing data is processed. And eliminating influence of sporadic fluctuation of sensor data by a range-variable mean filtering method, and ensuring real-time property of the data.

In the case where the first joint and the second joint are both revolute joints, for step S101, the predicting the relative motion trend between the first joint and the second joint may include: and determining that the first joint and the second joint have a tendency to approach in the horizontal direction when the swing joint of the first device and the swing joint of the second device meet a horizontal approach condition.

Wherein the horizontal approach condition may include at least one of: the revolute joint of the first device and the revolute joint of the second device enter a defined joint collision region; the rotary joint of the first device and the rotary joint of the second device enter respective critical collision risk areas simultaneously; the revolute joint of the first device entering a defined joint collision region of the first device and the revolute joint of the second device entering a critical collision risk region of the second device; the revolute joint of the second device entering a defined joint collision region of the second device and the revolute joint of the first device entering a critical collision risk region of the first device; the swivel joint of the first device and the swivel joint of the second device swivel in the same direction in a plurality of consecutive cycles, respectively.

The first critical collision point and the second critical collision point are the intersection points of the outline of the horizontal operation area of the first equipment and the outline of the horizontal operation area of the second equipment.

Next, the approaching horizontal direction trend is predicted based on the division rule of the horizontal work area.

The first step is to explain the horizontal operation area from two working conditions of the second equipment being the tower crane and the horizontal boom of the tower crane being higher than the boom end of the first equipment (case one) and the other case (case two).

For example, in the case where the second apparatus is a tower crane and the horizontal boom thereof is higher than the boom end of the first apparatus (e.g., crane) (case one), the horizontal operation area of the second apparatus is a smaller circle (i.e., a circle centered on the center of rotation and a horizontal distance from the center of rotation to the luffing trolley) corresponding to the tower crane shown in fig. 2, and the horizontal operation area of the first apparatus is a circle (i.e., a circle centered on the center of rotation and a horizontal projection length of the boom is a radius) corresponding to the crane shown in fig. 2. For the rest of cases, the definition of the horizontal operation areas of the first device and the second device is the same, and the horizontal operation areas are all circular areas with the respective rotation centers as the centers of circles and the horizontal lengths of the respective arm frames as the radii (for example, the horizontal operation area of the tower crane is a larger circle shown in fig. 2, and the horizontal operation area of the crane is still a circle corresponding to the crane shown in fig. 2).

And a second step of judging whether the two horizontal operation areas determined in the first step overlap, if so (if the distance between the rotation centers of the two devices is smaller than the sum of the respective radiuses of the two horizontal operation areas), determining the intersection point (namely, a first critical collision point a and a second critical collision point B as shown in fig. 2) according to the outline of the two horizontal operation areas, and determining that a quadrilateral area formed by the rotation center O1 of the first device, the first critical collision point a, the rotation center O2 of the second device and the second critical collision point B as shown in fig. 2 is a shared limited joint collision area. Then, according to the current speed of the rotary joint of the first device, determining an angle area (a sector O1CA and a sector O1C' B shown in fig. 2) rotated by the rotary joint when the rotary joint just enters a limited joint collision area during stable stopping, wherein the angle area is a critical collision risk area of the first device; similarly, an angular region (such as the sectors O2DA and O2D' B shown in fig. 2) through which the swing joint turns when just entering the defined joint collision region upon smooth stopping is determined according to the current speed of the swing joint of the second device, the angular region being a critical collision risk region of the second device.

And thirdly, determining whether the rotary joints of the first equipment and the rotary joints of the second equipment meet a horizontal approaching condition according to the rotary angles of the rotary joints of the first equipment and the second equipment, and if so, enabling the rotary joints of the two equipment to have approaching trend.

Then, based on the rule of division of the vertical work area, the approaching trend in the vertical direction is predicted (when the rectangles overlap, the vertical direction is considered to have a spatial overlap, and here, the prediction is performed before the rectangular spaces do not overlap). For the approach trend in the vertical direction, the analysis can be performed for the two conditions of fig. 3 and 4A (or fig. 4B).

In the case where the amplitude-changing joint end of the first device is higher than the upper side of the swing joint of the second device, the first joint is an amplitude-changing joint, and the second joint is a swing joint, for step S101, the predicting the relative movement trend between the first joint and the second joint may include: determining a vertical working area of the first equipment and a vertical working area of the second equipment; and determining that the first joint and the second joint have a tendency to approach in the vertical direction when the vertical working area of the first device and the vertical working area of the second device satisfy a first vertical approach condition.

The vertical working area of the first equipment is a rectangular area with the horizontal projection length of the luffing joint of the first equipment being long and the vertical distance from the tail end of the luffing joint of the first equipment to the lower side of the luffing joint of the second equipment being wide; and the vertical working area of the second equipment is a rectangular area with the horizontal projection length of the rotary joint of the second equipment being long and the vertical distance from the tail end of the rotary joint of the second equipment to the tail end of the lifting joint of the second equipment being wide.

Wherein the first vertical approach condition may include: the horizontal distance between the vertical working area of the first device and the vertical working area of the second device decreases over a plurality of consecutive cycles.

In an embodiment, the process of determining the vertical operation area specifically may be described in detail above, and if the first device is a crane and the second device is a tower crane, the rectangular area corresponding to the crane (the horizontal projection length of the boom of the crane is long and the vertical distance from the boom end of the crane to the lower side of the luffing trolley/horizontal boom of the tower crane is wide) is taken as the vertical operation area; the rectangular area corresponding to the tower crane (the length of the horizontal arm support of the tower crane is long and the vertical distance from the tail end of the horizontal arm support of the tower crane to the tail end of the lifting hook of the tower crane is wide) is taken as a vertical operation area, as shown in fig. 3. If the first device is a crane and the second device is a pumping device (e.g., a pump truck), the vertical working area of the crane remains unchanged, but the vertical working area of the pump truck is a rectangular area with the boom having a long horizontal projection length and a wide vertical distance from the boom end to the distribution pipe end (i.e., the length of the distribution pipe). Under the condition that vertical operation areas of two devices are determined, whether the boom of the crane and the horizontal boom (or the top end of the boom) of the tower crane meet a first vertical approaching condition or not is determined according to the vertical operation areas of the first device and the vertical operation areas of the second device, and if so, the boom of the crane and the horizontal boom (or the top end of the boom) of the tower crane are considered to have approaching trends.

In the case that the end of the luffing joint of the first device is lower than the lower side of the swing joint of the second device, the first joint is a luffing joint, and the second joint is a luffing joint or a lifting joint, for step S101, the predicting the relative motion trend between the first joint and the second joint may include: determining a vertical working area of the first equipment and a vertical working area of the second equipment; and determining that the first joint and the second joint have a tendency to approach in the vertical direction when the vertical working area of the first device and the vertical working area of the second device satisfy a second vertical approach condition.

The vertical working area of the first equipment is a rectangular area with the horizontal projection length of the luffing joint of the first equipment being long and the vertical distance from the tail end of the luffing joint of the first equipment to the tail end of the lifting joint of the first equipment being wide; and the vertical working area of the second equipment is a rectangular area with the horizontal distance from the rotation center of the second equipment to the outer side of the amplitude variation joint being long and the vertical distance from the amplitude variation joint of the second equipment to the tail end of the lifting joint of the second equipment being wide.

Wherein the second vertical approach condition may include: the vertical working area of the first apparatus and the vertical working area of the second apparatus overlap in a vertical direction but do not overlap in a horizontal direction (as shown in fig. 4A), and a horizontal distance between the vertical working area of the first apparatus and the vertical working area of the second apparatus decreases in a plurality of consecutive periods; or the vertical working area of the first apparatus and the vertical working area of the second apparatus overlap in the horizontal direction but do not overlap in the vertical direction (as shown in fig. 4B), and the vertical distance between the vertical working area of the first apparatus and the vertical working area of the second apparatus decreases in a plurality of consecutive periods.

In an embodiment, the process of determining the vertical operation area specifically may be described in detail above, and if the first device is a crane and the second device is a tower crane, the rectangular area corresponding to the crane is the vertical operation area thereof; the rectangular area corresponding to the tower crane is a vertical operation area, as shown in fig. 4A or fig. 4B. Correspondingly, the second vertical approaching condition specifically includes: the vertical working area of the crane and the vertical working area of the tower crane overlap in the vertical direction but do not overlap in the horizontal direction, and the horizontal distance from the hook end of the tower crane to the boom end of the crane decreases over a plurality of consecutive cycles; or the vertical operation area of the crane and the vertical operation area of the tower crane are overlapped in the horizontal direction but not overlapped in the vertical direction; and the vertical distance from the hook end of the tower crane to the boom end of the crane decreases over a plurality of consecutive cycles.

If the first device is a crane and the second device is a pumping device (e.g., a pump truck), the vertical working area of the crane remains unchanged, but the vertical working area of the pump truck is a rectangular area with the boom having a long horizontal projection length and a wide vertical distance from the boom end to the distribution pipe end (i.e., the length of the distribution pipe). Correspondingly, the second vertical approaching condition specifically includes: the vertical working area of the crane and the vertical working area of the pump truck overlap in a vertical direction but do not overlap in a horizontal direction, and the horizontal distance from the hook end of the pump truck to the boom end of the crane decreases over a plurality of consecutive cycles; or the vertical operation area of the crane and the vertical operation area of the pump truck are overlapped in the horizontal direction but not overlapped in the vertical direction; and the vertical distance from the hook end of the pump truck to the boom end of the crane decreases over a plurality of consecutive cycles.

Under the condition that the vertical operation areas of the two devices are determined, whether the luffing trolley or the lifting hook of the crane and the luffing trolley or the lifting hook of the tower crane meet a second vertical approaching condition or not is determined according to the vertical operation area of the first device and the vertical operation area of the second device, and if yes, the luffing trolley or the lifting hook of the crane and the luffing trolley or the lifting hook of the tower crane are considered to have approaching trend.

Step S102, determining a control strategy for the first joint and the second joint according to the speed of the first joint and the speed of the second joint and the joint relative distance between the first joint and the second joint when the relative movement trend indicates that the first joint and the second joint have a tendency to approach.

Wherein the joint relative distance may comprise: displacement of joint play (e.g., displacement of a lifting joint play), or linear displacement corresponding to an angle of joint play (e.g., linear displacement corresponding to an angle of a swivel joint play).

The control strategy may include a deceleration strategy, an avoidance strategy, an scram strategy, and/or a normal operation strategy, among others. Specifically, the deceleration strategy refers to that the running path of the device is unchanged, and the running speed of each joint of the device is smaller than the current speed thereof, for example, in the case that the current gear is a first gear, the running speed of each joint is limited to be within a second gear or higher. The avoidance strategy refers to that a certain joint of the device runs in the opposite direction of the current movement trend, and the running speed of each joint is smaller than the current speed of each joint. The scram strategy refers to stopping the operation of each joint of the equipment. The normal operation strategy refers to that all joints of the equipment operate along a current operation path at a current speed.

For step 102, the determining a control strategy for the first joint and the second joint may include: determining a control time threshold according to the speed of the first joint, the speed of the second joint, the total inertia parameters of the first equipment and the second equipment and a first corresponding relation, wherein the first corresponding relation is a corresponding relation among the first joint speed, the second joint speed, the control time and the inertia parameters; and determining the control strategy according to the control opportunity threshold and the joint relative distance between the first joint and the second joint.

According to the motion performance test data of the first equipment and the second equipment, a corresponding list of the first joint speed, the second joint speed, the inertia parameter and the control time (safety threshold) can be generated, and in the control process, the control time threshold can be dynamically adjusted by combining the measured speed of the first joint, the measured speed of the second joint and the total inertia parameter of the first equipment and the second equipment through a table lookup method. Of course, in another embodiment, according to the motion performance test data of the first device and the second device, a functional relationship between the first joint speed, the second joint speed, the inertia parameter and the control opportunity (safety threshold) may be generated, and in the control process, the control opportunity threshold may be dynamically adjusted through the functional relationship in combination with the (actually measured) speed of the first joint, the (actually measured) speed of the second joint and the total inertia parameter of the first device and the second device.

The speed of the first joint or the second joint can be directly detected by a sensor, or the speed of the first joint or the speed of the second joint can be indirectly obtained by processing sensor data, or the speed can be directly given by a controller for controlling the movement of the equipment.

In the case where the control opportunity threshold may include a first threshold, a second threshold, and a third threshold, the determining the control strategy may include: determining a deceleration strategy and an avoidance strategy for one of the first joint and the second joint as the first sub-control strategy and a normal operation strategy for the other joint as the second sub-control strategy, in the case that a joint relative distance between the first joint and the second joint is less than the first threshold and greater than or equal to the second threshold; determining a deceleration strategy and an avoidance strategy for one of the first joint and the second joint as the first sub-control strategy and a deceleration strategy for the other joint as the second sub-control strategy, in the case that a joint relative distance between the first joint and the second joint is less than the second threshold and greater than or equal to the third threshold; or if the joint relative distance between the first joint and the second joint is smaller than the third threshold, determining a scram strategy and a dodging strategy for one joint of the first joint and the second joint as the first sub-control strategy, and determining a scram strategy for the other joint as the second sub-control strategy.

Wherein the first threshold is greater than the second threshold and the second threshold is greater than the third threshold.

Specifically, the first threshold is a joint relative distance between the first joint and the second joint that runs for a first preset time at a speed threshold without collision. For example, the safety threshold value 1 corresponding to two specific joints refers to a joint relative distance at which the two specific joints having a tendency to approach move at a low speed for a first specific time (e.g., 15 s) without collision.

The second threshold is a joint relative distance between the first joint and the second joint which runs for a second preset time at the speed threshold without collision. The second preset time is smaller than the first preset time. For example, the safety threshold 2 corresponding to two specific joints refers to a joint relative distance at which the two specific joints having a tendency to approach move at a low speed for a second specific time (e.g., 10 s) without collision, wherein the second specific time may be smaller than the first specific time.

The third threshold is a joint relative distance at which the first joint and the second joint come to rest at respective current speeds without collision. Wherein the speed threshold is less than the lesser of the speed of the first joint and the speed of the second joint. For example, the safety threshold 3 for two particular joints refers to the relative distance of the joints at which the two particular joints have a tendency to approach from their respective current speeds of movement are stationary (e.g., in such a manner that the deceleration is the same or the deceleration changes less) to ensure that the hook does not rock within a preset range without collision.

The three safety thresholds described above vary with the (current running) speed of the two joints and the joint characteristics of the device itself.

From the above description, it is clear that the joint angle or the joint relative distance between two joints determines the need to take different anti-collision measures. When the joint angle or the joint relative distance between the two joints is greater than or equal to the first threshold value, collision risk can be reduced by means of one joint decelerating and avoiding and the other joint operating normally. When the joint angle or the joint relative distance between two joints is smaller than the first threshold value and larger than or equal to the second threshold value, the collision risk can be reduced by a way that one joint decelerates and dodges and the other joint decelerates and runs. When the joint angle or the joint relative distance between the two joints is smaller than the second threshold value and larger than or equal to the third threshold value, the collision risk can be reduced by means of sudden stop and avoidance of one joint and sudden stop of the other joint.

The control timing for two joints with a tendency to approach and the overall control strategy for the two joints (i.e., the control strategy includes a first sub-control strategy and a second sub-control strategy) can be determined by the three safety thresholds described above. However, it is not clear which joint the first sub-control strategy should be implemented for which joint the second sub-control strategy should be implemented for, as can be determined by the following.

For step S103, the controlling the first joint to execute a first sub-control strategy of the control strategies and the second joint to execute a second sub-control strategy of the control strategies may include: and controlling the first joint of the first equipment to execute the first sub-control strategy and controlling the second joint of the second equipment to execute the second sub-control strategy according to the construction method avoiding principle.

Wherein the first device is an empty load device and the second device is an empty load device; the first device is a low-speed device and the second device is a high-speed device; or the first device is a lifting device and the second device is a pumping device.

In this embodiment, since the spatial position and the motion characteristics of the device are changed during the operation (for example, when the device is lifted, the lifted object occupies a part of the operation space), and since the lifting rope is flexible, the stable control is difficult due to the shaking of the lifted object, it is necessary to specify which device is used to execute the corresponding sub-control strategy in combination with the construction method avoidance principle.

Wherein, the construction method avoiding principle comprises the following steps: avoiding heavy-load equipment by idle equipment according to the flexibility of equipment operation; according to the stability of equipment operation, the low-speed equipment avoids high-speed equipment; and/or avoiding the pumping equipment according to the time-efficient hoisting equipment for equipment operation. The term "dodging" may be interpreted herein as "priority" rather than "dodging policy". For example, "no-load device avoidance heavy-load device" may be understood as "priority of no-load device is lower than heavy-load device", "low-speed device avoidance high-speed device" may be understood as "priority of low-speed device is lower than high-speed device", and "lifting device avoidance pumping device" may be understood as "priority of lifting device is lower than pumping device".

When the joint angle or the joint relative distance between the two joints is smaller than the first threshold value and larger than or equal to the second threshold value, the joint of the idle equipment (or the low-speed equipment or the hoisting equipment) can be used for performing deceleration and avoidance, and the joint of the heavy-load equipment (or the high-speed equipment or the pumping equipment) can be used for performing normal operation to reduce collision risk. When the joint angle or the joint relative distance between the two joints is smaller than the second threshold value and larger than or equal to the third threshold value, the joint of the idle equipment (or the low-speed equipment or the hoisting equipment) can be used for performing deceleration and avoidance, and the joint of the heavy-load equipment (or the high-speed equipment or the pumping equipment) can be used for performing deceleration operation to reduce collision risk. When the joint angle or the joint relative distance between the two joints is smaller than the third threshold value, the sudden stop and avoidance can be performed through the joints of the idle equipment (or the low-speed equipment or the hoisting equipment), and the collision risk is reduced through the manner of performing the sudden stop through the joints of the heavy-load equipment (or the high-speed equipment or the pumping equipment).

Further, the hoisting/pumping situation is most preferably considered, the loading situation (no load, heavy load) is considered, and finally the speed (high speed, low speed) is considered, by default, but this is adjustable according to the specific site situation.

In the case that the principle cannot be judged, the joints of the equipment entering into the limited joint collision area from the back are decelerated and avoided, and the joints of the other equipment normally operate. And in other areas, all devices normally operate towards the target area.

In an embodiment, in a case that the swing joint of the first device and the swing joint of the second device respectively enter respective critical collision risk areas, the cooperative control method may further include: calculating a distance between any one of the plurality of feature points of the bounding box corresponding to any one joint of the first device and any one of the plurality of feature points of the bounding box corresponding to any one joint of the second device based on the center coordinates of the chassis of the first device, the center coordinates of the chassis of the second device, the coordinates of the plurality of feature points of the bounding box corresponding to each joint of the first device, and the coordinates of the plurality of feature points of the bounding box corresponding to each joint of the second device, when the coordinates of the bounding box corresponding to the swivel joint of the first device and the coordinates of the bounding box corresponding to the swivel joint of the second device indicate that there is an overlap between the horizontal operation area of the first device and the horizontal operation area of the second device; determining the approaching speed of the first joint and the second joint according to the minimum distance when the distance between the nearest two feature points on the bounding box corresponding to the first joint of the first device and the bounding box corresponding to the second joint of the second device is minimum and the minimum distance is smaller than a preset distance, wherein the approaching speed is the change rate of the minimum distance with time; and under the condition that the approach speed is smaller than 0, controlling the first joint and the second joint to execute an emergency stop strategy.

Specifically, in this embodiment, the motion trend is predicted based on the joint space distance calculation (based on the device model and the joint parameters, the distance between the device joints is rapidly detected, and the joint number and the distance closest to the device joint are output), and the joint operation is controlled by adopting the scram strategy when the minimum distance between the two joints is very close and has a close trend.

As shown in fig. 5, the process of predicting the movement tendency based on the joint space distance calculation may include the following steps S501 to S506.

In step S501, cluster equipment composition and parameters are determined.

The common equipment for the construction site comprises a tower crane, a pump truck and the like, and has the characteristics of large operation range, large possibility of overlapping operation space, changeable arm support structure, complex motion state, large motion inertia and the like.

And (3) tower crane: base position (x, y, z), standard knot size, standard knot number, boom size, lifting wire rope length, hook size.

And (3) a crane: chassis position (x, y, z), chassis size, boom number, boom size, lifting wire rope length, hook size.

And (3) a pump truck: chassis position (x, y, z), chassis size, boom number, boom size, distribution pipe size.

In this embodiment, the cluster device is determined to be one device (e.g., device a) and another device (e.g., device B) of the devices and the corresponding parameters.

Step S502, abstract each type of equipment into a plurality of three-dimensional bounding boxes according to a specific rule.

The three-dimensional bounding box comprises a cubic bounding box and a cylindrical bounding box.

According to the motion characteristics and the structural characteristics of different types of equipment, a set of bounding box abstraction rules are established, so that the safe operation of the equipment can be ensured, and the occupation of space resources can be reduced.

(1) Tower crane

The actual composition comprises parts of a foundation, a tower body, a jacking, a rotation, a lifting, a balance arm, a lifting arm, a luffing trolley, a tower top, a cab and the like, and the abstract composition comprises the following components: vertical ground bounding boxes, parallel ground bounding boxes, sling bounding boxes, and sling bounding boxes, as shown in fig. 6.

Vertical ground bounding box: and establishing a rectangular bounding box from the ground to the tower top height H, and bounding a maximum projection plane of a foundation, a tower body, a lifting part, a rotating part and a tower top part. The part is not fixed with the bounding box, and the tower crane can not send motion during operation;

Parallel ground bounding boxes: establishing a rectangular bounding box which surrounds the balance arm, the crane arm, the luffing trolley and the cab from the tail of the balance arm to the tail end of the crane arm, wherein the bounding box is a rotatable part, and the rectangular bounding box is equivalent to a rotating point with a rotation center as a rotating point and rotates in parallel with the ground during actual operation;

the lifting rope encloses the box: establishing a cylindrical bounding box from the amplitude-variable trolley to the bottom height h of the lifting hook, surrounding the steel wire rope and the ground projection surface of the lifting hook, and considering the swinging characteristic of the lifting hook, and also establishing a conical bounding box;

hanging object bounding box: according to the dimension information of the suspended object, a cylindrical bounding box is established in consideration of rotation characteristics.

(2) Crane with crane body

The practical composition comprises two parts of getting off and getting on, wherein the getting on consists of a rotary mechanism part, a luffing mechanism part, a lifting mechanism part and a telescopic mechanism part. The abstract composition comprises: the drop box, boom box, hoist rope hook box, hoist object box are shown in fig. 7.

Get off bounding box: on the premise of completely unfolding the supporting legs, a rectangular bounding box which surrounds the chassis, the cab and the projection surface of the supporting legs of the automobile is established from the ground to the height h above the rotary platform.

Arm support bounding box: the method comprises the steps of establishing a rectangular bounding box which surrounds a main arm and an auxiliary arm, wherein the bounding box is a rotatable part, and when in actual operation, the rectangular bounding box is equivalently rotated by the center of a rotary platform and can extend outwards along the arm support direction, the rotation angle is determined by a rotation angle and an amplitude angle, and the rectangular length is determined by the length of the main arm and the telescopic length of the auxiliary arm.

The lifting rope encloses the box: and establishing a cylindrical bounding box from the tail end of the arm support to the bottom height h of the lifting hook, surrounding the projection surface of the steel wire rope and the lifting hook towards the ground, and considering the swinging characteristic of the lifting hook, and also establishing a conical bounding box.

(3) Pump truck

The practical composition comprises two parts of getting off and getting on, wherein the getting on consists of a rotary multi-joint arm support and a material distribution pipe. The abstract composition comprises: the lower car bounding box, the boom 1 bounding box, the boom 2 bounding box, the … boom N bounding box and the distribution pipe bounding box are shown in fig. 8.

Arm support bounding box: each arm support is independently abstracted into a bounding box, the bounding box is fixed in size, but the bounding box can rotate up and down around the tail end of the upper arm support, rotate left and right around the rotation center and move along with the tail end of the upper arm support, and the bounding box of the first arm support rotates left and right around the rotation center and up and down.

Cloth pipe bounding box: and (3) establishing a cylindrical bounding box perpendicular to the ground from the tail end of the last arm support.

In step S503, a bounding box list is built in real time.

The bounding box list comprises size, position and gesture information. The important description here is that the composition of coordinate points required by determining the sizes, positions and attitudes of different bounding boxes converts the rotation and scaling of the bounding box into the rotation and scaling of the corresponding coordinate points.

Each device contains a set of bounding boxes, the relevant information of which can be used with the coordinate information of a plurality of feature points of the bounding box. For example, the cube bounding box may be represented by 4 feature points (x 1, y1, z 1), (x 2, y2, z 2), (x 3, y3, z 3), (x 4, y4, z 4) shown in fig. 9, and the column bounding box may be represented by 3 feature points (x 1, y1, z 1), (x 2, y2, z 2), (x 3, y3, z 3) shown in fig. 9. That is, the size, position, and posture information of the bounding box can be accurately characterized by a plurality of feature points of the bounding box.

And acquiring the position and posture information of the equipment chassis and each joint in real time, and converting the information into corresponding bounding box size, position and posture information, namely converting the corresponding bounding box size, position and posture information into coordinate values of a plurality of characteristic points of the bounding box.

Step S504, judging whether the horizontal operation areas among the devices have overlapping possibility, if so, executing step S505; otherwise, step S503 is executed.

And calculating whether the horizontal operation ranges overlap or not, and performing a subsequent collision detection flow only on equipment with overlapped operation ranges, so that the calculation complexity is reduced to the greatest extent.

In step S505, the minimum distance between the two devices is calculated based on the bounding box list.

Device a has four equivalent bounding boxes a ₁ 、a ₂ 、a ₃ 、a ₄ The method comprises the steps of carrying out a first treatment on the surface of the Device B has four equivalent bounding boxes B ₁ 、b ₂ 、b ₃ 、b ₄ The method comprises the steps of carrying out a first treatment on the surface of the Bounding box a ₁ And bounding box b ₁ The distance between the two points is denoted as D _a1-b1 。

Calculated, bounding box a _i And bounding box b _j The shortest distance between the closest two feature points on the bounding box is the smallest distance (of the distances between the closest two feature points on any two bounding boxes), then the shortest distance between devices A and B is denoted as min _i，j D _ai-bj 。

That is, each bounding box of the device a is used to sequentially calculate the distances between the device a and all bounding boxes of the device B, and the minimum distance between the equivalent bounding boxes of the two devices is the actual minimum distance between the two devices, and the two bounding boxes with the minimum distance are joints which are likely to collide.

The distance between the common cubes and the column bounding boxes is calculated once only by 1 millisecond, the equivalent bounding boxes of the tower crane and the crane are generally 4, the equivalent bounding boxes of the seven-joint pump truck are generally 8, collision detection between the three is carried out once, 4 x 4+4 x 8+4 x 8=80 times of calculation is needed, and the time can be controlled within 100 milliseconds.

In step S506, the relative motion trend between the devices is analyzed.

When the minimum distance between the devices is smaller than the preset distance (for example, 10 meters), differentiating the minimum distance between the devices to obtain the device approaching speed, V _AB ＝d(min _i，j D _ai-bj ) /dt. When the approach speed is smaller than 0, the distance between the devices is considered to have a decreasing trend, namely the joints corresponding to the bounding boxes have an approach trend, and the corresponding risk joints are recorded.

The cooperative control process (avoidance strategy based on the relative spatial states between devices) is now summarized and described below.

After two devices enter a critical collision risk area, the two devices are controlled to cooperatively operate in a shared area according to the movement trend of the area and each joint, and the two devices are mainly controlled in a combined mode through four states of speed reduction, avoidance, scram and normal operation.

For long-arm-frame multi-joint engineering machinery equipment, the movement of each joint of the equipment is a process of gradual acceleration, smooth running and gradual deceleration, so when the equipment is in a critical collision risk area, the relative movement trend of two joints from the two equipment needs to be predicted. If the joint is judged to have a approaching trend, calculating a corresponding safety threshold according to the movement speed of the joint; when two devices enter the corresponding threshold value interval, the corresponding devices are controlled to adjust the joint gesture according to the construction method avoidance principle, and the devices are avoided towards the opposite direction of the current movement trend, so that the devices are prevented from collision when entering the limited joint collision area.

Thus, the cooperative control platform shown in fig. 10 performs an anti-collision cooperative control process using the process described in the above embodiments based on a multi-machine finite state decision process of a decision tree model: based on the avoidance principle and method of the space position (based on the equipment space state classification of the dynamic shared space, the dynamic safety threshold value calculation of the motion state of each joint of the equipment, and the prediction of the motion trend of the joints of the equipment), the automatic control equipment cooperates with the site construction method to perform work (namely, control each equipment to perform tasks or perform actions such as deceleration, flexibility, avoidance, emergency stop and the like), so that the engineering mechanical equipment with larger motion inertia can be ensured to timely detect collision risk, and safety avoidance measures are adopted. And, the computation complexity of each of the above embodiments is low.

In summary, the present invention creatively predicts the relative motion trend between the first joint of the first device and the second joint of the second device; then, under the condition that the relative movement trend shows that the first joint and the second joint have approaching trend, determining a control strategy for the first joint and the second joint according to the speed of the first joint and the speed of the second joint and the joint relative distance between the first joint and the second joint; and finally, according to a construction method avoiding principle, controlling the first joint to execute a first sub-control strategy in the control strategy and the second joint to execute a second sub-control strategy in the control strategy so as to prevent the first equipment from colliding with the second equipment. Therefore, the method and the device are based on the prediction of the movement trend of the joints of the equipment, the calculation of the dynamic safety threshold value of the movement state of each joint of the equipment and the avoidance principle and method of the site construction method, and the automatic control equipment cooperatively works, so that the engineering mechanical equipment with larger movement inertia can be ensured to timely detect collision risk, and safety avoidance measures are adopted.

An embodiment of the present invention further provides a cooperative control system, including: trend predicting means for predicting a relative movement trend between a first joint of a first device and a second joint of a second device; the strategy determining device is used for determining a control strategy for the first joint and the second joint according to the speed of the first joint and the speed of the second joint and the joint relative distance between the first joint and the second joint when the relative movement trend shows that the first joint and the second joint have a approaching trend, wherein the control strategy comprises a deceleration strategy, an avoidance strategy, an emergency stop strategy and/or a normal operation strategy; and the control device is used for controlling the first joint to execute a first sub-control strategy in the control strategies and the second joint to execute a second sub-control strategy in the control strategies according to the construction method avoiding principle so as to prevent the first equipment from colliding with the second equipment.

An embodiment of the present invention further provides a group of engineering machines, including: a first device; a second device; and the cooperative control system is used for controlling the operation of the first equipment and the second equipment according to the cooperative control method.

The preferred embodiments of the present invention have been described in detail above with reference to the accompanying drawings, but the present invention is not limited to the specific details of the above embodiments, and various simple modifications can be made to the technical solution of the present invention within the scope of the technical concept of the present invention, and all the simple modifications belong to the protection scope of the present invention.

In addition, the specific features described in the above embodiments may be combined in any suitable manner without contradiction. The various possible combinations of the invention are not described in detail in order to avoid unnecessary repetition.

Moreover, any combination of the various embodiments of the invention can be made without departing from the spirit of the invention, which should also be considered as disclosed herein.

Claims

1. A cooperative control method, characterized in that the cooperative control method comprises:

Predicting the relative movement trend between a first joint of a first device and a second joint of a second device according to the movement pose of the first joint and the second joint;

determining a control strategy for the first joint and the second joint according to the speed of the first joint and the speed of the second joint and the joint relative distance between the first joint and the second joint under the condition that the relative movement trend shows that the first joint and the second joint have an approaching trend, wherein the control strategy comprises a deceleration strategy, an avoidance strategy, an scram strategy and/or a normal operation strategy; and

and according to a construction method avoiding principle, controlling the first joint to execute a first sub-control strategy in the control strategy and the second joint to execute a second sub-control strategy in the control strategy so as to prevent the first equipment from colliding with the second equipment.

2. The cooperative control method according to claim 1, characterized in that the cooperative control method further comprises:

determining a horizontal operation area of the first equipment and a horizontal operation area of the second equipment;

Determining a limited joint collision area shared by the first equipment and the second equipment and a critical collision risk area of each of the first equipment and the second equipment under the condition that the horizontal operation area of the first equipment and the horizontal operation area of the second equipment overlap; and

in the case where the revolute joint of the first device and the revolute joint of the second device enter respective critical collision risk regions, performing the step of predicting the relative movement tendency between the first joint and the second joint,

3. The cooperative control method according to claim 2, wherein, in the case where the first joint and the second joint are both revolute joints, the predicting the relative movement tendency between the first joint and the second joint includes:

determining that there is a tendency for the first joint to approach the second joint in a horizontal direction if the swivel joint of the first device and the swivel joint of the second device satisfy a horizontal approach condition, wherein the horizontal approach condition includes at least one of:

the revolute joint of the first device and the revolute joint of the second device enter a defined joint collision region;

the rotary joint of the first device and the rotary joint of the second device enter respective critical collision risk areas simultaneously;

the revolute joint of the first device entering a defined joint collision region of the first device and the revolute joint of the second device entering a critical collision risk region of the second device;

the revolute joint of the second device entering a defined joint collision region of the second device and the revolute joint of the first device entering a critical collision risk region of the first device;

The swivel joint of the first device and the swivel joint of the second device swivel in the same direction in a plurality of consecutive cycles, respectively.

4. A cooperative control method as claimed in claim 3, wherein the first critical collision point and the second critical collision point are intersections of a contour of a horizontal operation area of the first apparatus and a contour of a horizontal operation area of the second apparatus.

5. The cooperative control method according to claim 1 or 2, wherein in the case where the amplitude-changing joint end of the first device is higher than the upper side of the swing joint of the second device, the first joint is an amplitude-changing joint, and the second joint is a swing joint, the predicting the relative movement tendency between the first joint and the second joint includes:

determining a vertical working area of the first equipment and a vertical working area of the second equipment; and

in the case where the vertical working area of the first apparatus and the vertical working area of the second apparatus satisfy a first vertical approach condition, determining that there is an approach trend of the first joint and the second joint in the vertical direction,

6. The cooperative control method of claim 5, wherein the first vertical approach condition comprises:

the horizontal distance between the vertical working area of the first device and the vertical working area of the second device decreases over a plurality of consecutive cycles.

7. The cooperative control method according to claim 1 or 2, wherein in the case where the amplitude joint end of the first device is lower than the swing joint lower side of the second device, the first joint is an amplitude joint, and the second joint is an amplitude joint or a lifting joint, the predicting the relative movement tendency between the first joint and the second joint includes:

in the case where the vertical working area of the first apparatus and the vertical working area of the second apparatus satisfy a second vertical approach condition, determining that there is an approach trend of the first joint and the second joint in the vertical direction,

8. The cooperative control method of claim 7, wherein the second vertical approach condition comprises:

the vertical working area of the first apparatus and the vertical working area of the second apparatus have overlap in a vertical direction but do not overlap in a horizontal direction, and a horizontal distance between the vertical working area of the first apparatus and the vertical working area of the second apparatus decreases in a plurality of consecutive periods; or alternatively

The vertical working area of the first apparatus and the vertical working area of the second apparatus have overlap in a horizontal direction but do not overlap in a vertical direction, and a vertical distance between the vertical working area of the first apparatus and the vertical working area of the second apparatus decreases in a plurality of consecutive periods.

9. The cooperative control method of claim 1, wherein the determining a control strategy for the first joint and the second joint comprises:

determining a control time threshold according to the speed of the first joint, the speed of the second joint, the total inertia parameters of the first equipment and the second equipment and a first corresponding relation, wherein the first corresponding relation is a corresponding relation among the first joint speed, the second joint speed, the control time and the inertia parameters; and

And determining the control strategy according to the control time threshold and the joint relative distance between the first joint and the second joint.

10. The cooperative control method of claim 9, wherein, in the case where the control opportunity threshold includes a first threshold, a second threshold, and a third threshold, the determining the control strategy includes:

determining a deceleration strategy and an avoidance strategy for one of the first joint and the second joint as the first sub-control strategy and a normal operation strategy for the other joint as the second sub-control strategy, in the case that a joint relative distance between the first joint and the second joint is less than the first threshold and greater than or equal to the second threshold;

determining a deceleration strategy and an avoidance strategy for one of the first joint and the second joint as the first sub-control strategy and a deceleration strategy for the other joint as the second sub-control strategy, in the case that a joint relative distance between the first joint and the second joint is less than the second threshold and greater than or equal to the third threshold; or alternatively

Determining a scram strategy and a dodging strategy for one of the first and second joints as the first sub-control strategy and a scram strategy for the other joint as the second sub-control strategy in the case that a joint relative distance between the first and second joints is less than the third threshold,

11. The cooperative control method of claim 10, wherein the first threshold is a joint relative distance at which the first joint and the second joint travel a first predetermined time without collision at a speed threshold,

the second threshold is a joint relative distance between the first joint and the second joint running at the speed threshold for a second preset time without collision, wherein the second preset time is smaller than the first preset time, and

the third threshold is a relative distance between the first joint and the second joint which are decelerated and stopped at respective current speeds without collision,

wherein the speed threshold is less than the lesser of the speed of the first joint and the speed of the second joint.

12. The cooperative control method of claim 10, wherein the controlling the first joint to execute a first sub-control strategy of the control strategies and the second joint to execute a second sub-control strategy of the control strategies comprises:

according to the construction method avoiding principle, the first joint of the first equipment is controlled to execute the first sub-control strategy and the second joint of the second equipment is controlled to execute the second sub-control strategy,

13. The cooperative control method according to claim 2, wherein in the case where the swing joint of the first device and the swing joint of the second device respectively enter respective critical collision risk areas, the cooperative control method further includes:

calculating a distance between any one of the plurality of feature points of the bounding box corresponding to any one joint of the first device and any one of the plurality of feature points of the bounding box corresponding to any one joint of the second device based on the center coordinates of the chassis of the first device, the center coordinates of the chassis of the second device, the coordinates of the plurality of feature points of the bounding box corresponding to each joint of the first device, and the coordinates of the plurality of feature points of the bounding box corresponding to each joint of the second device, when the coordinates of the bounding box corresponding to the swivel joint of the first device and the coordinates of the bounding box corresponding to the swivel joint of the second device indicate that there is an overlap between the horizontal operation area of the first device and the horizontal operation area of the second device;

Determining the approaching speed of the first joint and the second joint according to the minimum distance when the distance between the nearest two feature points on the bounding box corresponding to the first joint of the first device and the bounding box corresponding to the second joint of the second device is minimum and the minimum distance is smaller than a preset distance, wherein the approaching speed is the change rate of the minimum distance with time; and

and under the condition that the approach speed is smaller than 0, controlling the first joint and the second joint to execute an emergency stop strategy.

14. The cooperative control method according to claim 2 or 13, wherein in the case where the amplitude joint end of the first apparatus is higher than the swing joint upper side of the second apparatus, the horizontal operation area of the first apparatus is a circular area centered on the swing center of the first apparatus and centered on the horizontal projection length of the amplitude joint, and the horizontal operation area of the second apparatus is a circular area centered on the swing center of the second apparatus and centered on the horizontal distance of the swing center to the boom end; or alternatively

When the tail end of the luffing joint of the first equipment is lower than the lower side of the turning joint of the second equipment, the horizontal operation area of the first equipment is a circular area taking the turning center of the first equipment as a circle center and taking the horizontal projection length of the arm support as a radius, and the horizontal operation area of the second equipment is a circular area taking the turning center of the second equipment as a circle center and taking the horizontal distance from the turning center to the outer side of the luffing joint as a radius.

15. A cooperative control system, the cooperative control system comprising:

trend predicting means for predicting a relative movement trend between a first joint of a first device and a second joint of a second device;

the strategy determining device is used for determining a control strategy for the first joint and the second joint according to the speed of the first joint and the speed of the second joint and the joint relative distance between the first joint and the second joint when the relative movement trend shows that the first joint and the second joint have a approaching trend, wherein the control strategy comprises a deceleration strategy, an avoidance strategy, an emergency stop strategy and/or a normal operation strategy; and

and the control device is used for controlling the first joint to execute a first sub-control strategy in the control strategies and the second joint to execute a second sub-control strategy in the control strategies according to the construction method avoiding principle so as to prevent the first equipment from colliding with the second equipment.

16. A group of construction machines, the group of construction machines comprising:

a first device;

a second device; and

A cooperative control system for controlling the operation of the first device and the second device according to the cooperative control method of any one of claims 1 to 14.