Break-in positioning system and method capable of automatically following target point

Abstract

The invention discloses a forcible entry positioning system and method capable of automatically following a target point, the system mainly comprises a holder, a laser range finder, a rotary encoder, a controller and a holder control handle, and the system is mounted on a rotary platform of forcible entry engineering machinery. After laser forcible entry point indication is completed by operating the holder to rotate, once a forcible entry positioning system detects a platform rotation signal, a new collected platform rotation angle value is automatically assigned to a target following algorithm, a new holder rotation angle and a new pitching angle value are calculated and assigned to the holder, a laser range finder is driven to rotate in a following mode, and the target following algorithm is started. Therefore, before the next forcible entry indication point is calibrated, the current forcible entry laser indication point is kept unchanged all the time, the next positioning operation time is shortened, and the rescue efficiency is improved.

Description

Technical Field

[0001] This invention relates to the field of demolition-resistant engineering machinery technology, specifically a demolition positioning system and method that automatically follows a target point. Background Technology

[0002] Demolition-resistant engineering machinery can be used for rescue operations in open spaces and large-scale structural collapses. Currently, some equipment is equipped with a laser pointing system for demolition points, used to mark demolition points and calculate their coordinates, facilitating automatic control of the demolition arm and rotating platform to proceed with the demolition. However, some current demolition-resistant engineering machinery uses laser rangefinders to pinpoint demolition targets. Since these rangefinders are mounted on the rotating platform, once the platform rotates to begin demolition, the gimbal and laser rangefinder rotate with it, causing the planned laser target pointing point to be lost. This leads to several problems: first, since demolition points are usually close together, losing the laser point increases the time needed for the next positioning operation, reducing rescue efficiency; second, it cannot directly display the positional error between the set target point and the actual demolition point, lacking accurate visual feedback, increasing the time required to judge the accuracy of demolition positioning in certain situations; and third, it does not provide an algorithm based on the spatial coordinates of the laser point from the gimbal laser rangefinder. Summary of the Invention

[0003] The purpose of this invention is to provide an automatic target-following demolition positioning system and method. Once the rotation signal of the rotating platform of the demolitionable construction machinery is detected, the newly collected rotation angle value is automatically assigned to the target following algorithm, the new circumferential rotation angle and pitch angle value of the gimbal are calculated and assigned to the gimbal, and the laser rangefinder is driven to follow the rotation, thereby ensuring that the current demolition laser indicator point remains unchanged before the next demolition indicator point is calibrated.

[0004] To achieve the above objectives, the present invention employs the following technical solution:

[0005] This invention provides an automatic target-following demolition positioning system, which is installed on the rotating platform of demolition-capable construction machinery. The demolition positioning system includes: a laser rangefinder, a gimbal, an angle encoder, a controller, and a gimbal control handle.

[0006] The angle encoder is used to measure the rotation angle of the rotary platform in real time and feed it back to the controller;

[0007] The controller is used to calculate the circumferential rotation angle and pitch angle that the gimbal should maintain based on the rotation angle of the rotating platform and the coordinates of the laser demolition point located by the laser rangefinder, and assign the values ​​to the gimbal.

[0008] Preferably, the laser rangefinder is mounted on the gimbal, which has the functions of circumferential rotation and pitch rotation, driving the laser rangefinder to perform circumferential rotation and pitch rotation; the laser rangefinder is used to emit laser light and measure the distance between the laser point and the emission point.

[0009] Preferably, the gimbal control handle is used to operate the gimbal to perform circumferential rotation and pitch rotation, so as to drive the laser rangefinder to emit a laser beam to the laser demolition point.

[0010] This invention also provides an automatic target-point following demolition positioning method, implemented based on the aforementioned automatic target-point following demolition positioning system, the method comprising:

[0011] Define the coordinate system and initial state of the demolition positioning system, and define the gimbal to first rotate circumferentially and then rotate in pitch.

[0012] The gimbal is rotated to complete the laser demolition point marking and positioning, and the rotation angle of the current rotating platform, the circumferential rotation angle of the gimbal, and the pitch angle of the gimbal are read.

[0013] Calculate the coordinates of the laser breaking point S after the laser beam length L is emitted by the laser rangefinder;

[0014] After the demolition operation, if the rotating platform rotates, the circumferential rotation angle and pitch angle that the gimbal should maintain are calculated based on the rotation angle of the rotating platform in the two previous operations and the coordinates of the laser demolition point, and then assigned to the gimbal.

[0015] Preferably, the coordinate system defining the demolition positioning system includes:

[0016] Let the front of the demolishable construction machinery be X+, the left side of the demolishable construction machinery be Y+, the rotation axis of the slewing platform be Z, the upward direction be Z+, and the intersection of the upper plane of the slewing bearing of the demolishable construction machinery and the rotation axis be the origin O.

[0017] Preferably, the initial state of the demolition positioning system is defined as follows:

[0018] Initially, the rotation angle of the slewing platform of the demolition machinery is 0°, and the rotation angle range is -180° to +180°. From the Z+ top view, counterclockwise is positive and clockwise is negative.

[0019] The position of the gimbal relative to the demolition machinery remains unchanged, the coordinate direction is consistent with the demolition positioning system, the circumferential rotation angle is 0°, and the circumferential rotation angle range is -180°~+180°. From the Z+ top view, counterclockwise is positive and clockwise is negative.

[0020] The pitch angle is 0°, and the pitch angle range is -90° to +90°. From the Y+ perspective, counterclockwise is positive and clockwise is negative.

[0021] The laser beam is directed at X+.

[0022] Preferably, the coordinates of the laser breaking point S after calculating the length L of the laser beam emitted by the laser rangefinder include:

[0023] X S =I*cosW-J*sinW+(R*cosE-T*sinE)*cosW*cosP-(R*sinE+T*cosE)*sinW-B*cosW*sinP+L*(cosE*cosW*cosP-sinE*sinW*cosP-cosE*sinW*sinP);

[0024] Y S =I*sinW+J*cosW+(R*cosE-T*sinE)*sinW*cosP+(R*sinE+T*cosE)*cosW-B*sinW*sinP+L*(cosE*sinW*cosP+sinE*cosW*cosP+cosE*cosW*sinP);

[0025] Z S =K + B * cosP + L * sinP;

[0026] Among them, X S Y S Z S These are the coordinates of the laser demolition point S, I, J, K, and the coordinates of the intersection point V of the gimbal's base center axis and pitch rotation axis relative to point O in the initial state. R, T, B are the coordinates of the laser ranging emission point Q relative to point V in the initial state. W is the rotation angle of the current demolitionable engineering machinery rotary platform, and E and P are the current circumferential rotation angle and pitch angle of the gimbal.

[0027] Preferably, the calculation of the circumferential rotation angle and pitch angle that the gimbal should maintain based on the rotation angle of the two rotating platforms and the coordinates of the laser demolition point includes: E'=arctan2(α,β), , , P'=arcsin((Z S -KB*cosP) / L);

[0028] Where arctan2(α,β) represents the angle between the line connecting point (α,β) and the origin and the positive X-axis, and E' and P' are the circumferential rotation angle and pitch angle that the gimbal should maintain.

[0029] Preferably, the method further includes,

[0030] If the calculated circumferential rotation angle and pitch angle that the gimbal should maintain exceed the corresponding range, then the boundary value of the nearest range shall be taken as the circumferential rotation angle and pitch angle that the gimbal should maintain.

[0031] The beneficial effects of the technical solution of this invention are as follows:

[0032] This invention provides an automatic target-following demolition positioning system. After laser demolition point indication is completed by manipulating the gimbal rotation, once the laser indication system detects the rotation signal of the slewing platform of the demolitionable construction machinery, it automatically assigns the newly collected rotation angle value to the target following algorithm. The algorithm calculates the new circumferential rotation angle and pitch angle values ​​of the gimbal, and then assigns them to the gimbal via the controller, causing the laser rangefinder to rotate in a following manner. This ensures that the current demolition laser indication point remains unchanged until the next demolition indication point is calibrated, reducing the time for the next positioning operation and improving rescue efficiency. Simultaneously, it directly displays the positional error between the set target point and the actual demolition point, forming an accurate visual closed-loop feedback, reducing the time required to judge the accuracy of demolition positioning in some working conditions. The algorithm based on the spatial coordinates of the laser point of the gimbal laser rangefinder provided by this invention supports subsequent precise and intelligent control of the boom. Attached Figure Description

[0033] Figure 1 A schematic diagram of the structure of an automatic target-following demolition positioning system provided by the present invention;

[0034] Figure 2 A schematic diagram of the laser-indicating target point provided by the present invention;

[0035] Figure 3 This is a flowchart illustrating the target point following control method provided by the present invention. Detailed Implementation

[0036] The specific embodiments of the present invention will be described in further detail below with reference to the accompanying drawings and examples. The embodiments described below with reference to the accompanying drawings are illustrative and intended to explain the present invention, and should not be construed as limiting the present invention.

[0037] In the description of this invention, it should be noted that the terms "upper", "lower", "front", "rear", "end", "bottom", "side", etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are only for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the device or component referred to must have a specific orientation. Therefore, they should not be construed as limiting this invention.

[0038] In this invention, unless otherwise explicitly specified and limited, the terms "connection," "installation," etc., should be interpreted broadly. For example, they can refer to a fixed connection, a direct connection, or a connection through an intermediate medium. Those skilled in the art can understand the specific meaning of these terms in this invention according to the specific circumstances.

[0039] Secondly, the term "an embodiment" or "embodiment" as used in this invention refers to a specific feature, structure, or characteristic that can be included in at least one implementation of this invention. The phrase "in one embodiment" appearing in different places in this specification does not necessarily refer to the same embodiment, nor is it a single or selective embodiment that excludes other embodiments.

[0040] The specific embodiments of the present invention will be described in further detail below with reference to the accompanying drawings. The following examples are for illustrative purposes only and are not intended to limit the scope of the invention.

[0041] See Figure 1 The present invention provides an automatic target-following demolition positioning system, which mainly includes a laser rangefinder 1-5, a gimbal 1-4, an angle encoder 1-2, a controller 1-3 and a gimbal control handle 1-6. The entire system is installed on the rotary platform 1-1 of the demolition machinery. During the demolition movement, the entire rotary platform 1-1 can rotate relative to the chassis through the slewing bearing.

[0042] In this invention, the angle encoder 1-2 is used to measure the rotation angle of the rotary platform 1-1 in real time and feed it back to the controller 1-3;

[0043] The laser rangefinder 1-5 is mounted on the gimbal 1-4, which has the functions of circumferential rotation and pitch rotation, and can drive the laser rangefinder 1-5 to perform circumferential rotation and pitch rotation; the laser rangefinder 1-5 can emit laser and measure the distance between the laser point and the emission point.

[0044] It should be noted that the gimbal 1-4 base is preferably installed on the upper surface of the cover of the rotary platform 1-1, or it can be installed on the bracket on the frame.

[0045] The gimbal control handles 1-6 are used to operate the gimbal 1-4 to rotate in all directions and in pitch, so as to drive the laser rangefinder 1-5 to emit a laser beam to the laser demolition point.

[0046] It should be noted that the gimbal control handles 1-6 can be reused using motion handles from robotic arms or other similar devices.

[0047] The controller 1-3 is used to process the detection or control signals transmitted and received by the laser rangefinder 1-5, the gimbal 1-4, the angle encoder 1-2, and the gimbal control handle 1-6.

[0048] Using the above-mentioned demolition positioning system, after the laser demolition point is marked and positioned by manipulating the gimbal to rotate, once the rotation signal of the rotating platform is detected, the angle encoder automatically sends the collected rotation angle value of the rotating platform to the controller.

[0049] The controller incorporates a target-following algorithm. This algorithm calculates the circumferential and pitch angles of the gimbal and assigns them to the gimbal to drive the laser rangefinder in a following rotation. This ensures that the current laser breaching point remains unchanged until the next laser breaching point is calibrated. Based on this, the time for subsequent positioning operations is reduced, improving rescue efficiency. By directly displaying the positional error between the set target point and the actual breaching point, accurate visual feedback is provided, reducing the time required to judge the accuracy of breaching positioning in certain working conditions.

[0050] It should be noted that other types of gimbals and laser rangefinders can also be used.

[0051] Based on the aforementioned automatic target-point following demolition positioning system, this invention also provides an automatic target-point following demolition positioning method, see [link to relevant documentation]. Figure 3 This includes the following steps:

[0052] 1) Define the coordinate system and initial state of the demolition positioning system.

[0053] In this invention, see Figure 2 Let the front side of the demolishable engineering machinery be X+, the left side of the demolishable engineering machinery be Y+, the rotation axis of the slewing platform be Z, and the upward direction be Z+; the intersection of the upper plane of the slewing bearing of the demolishable engineering machinery and the rotation axis is the origin O (0,0,0).

[0054] In this invention, a gimbal is mounted on the upper plane (parallel to the XY plane) of the scaffold of the detachable engineering machinery, and the gimbal base coincides with the upper plane of the scaffold. The gimbal has circumferential rotation and pitch rotation functions. A laser rangefinder is mounted on the gimbal loading platform, and its firing direction changes as the detachable engineering machinery platform rotates, the gimbal rotates circumferentially, and pitches. Before each start-up of the demolition machinery, the initial state is defined as follows: the rotation angle of the demolition machinery's rotating platform is 0° (rotation angle range is -180° to +180°, viewed from Z+, counterclockwise is positive, clockwise is negative), the position of the gimbal relative to the demolition machinery remains unchanged, the coordinate direction is consistent with the system coordinates, the circumferential rotation angle is 0° (circumferential rotation angle range is -180° to +180°, viewed from Z+, counterclockwise is positive, clockwise is negative), the pitch angle is 0° (pitch angle range is -90° to +90°, viewed from Y+, counterclockwise is positive, clockwise is negative), and the laser direction is X+.

[0055] It should be noted that the limits of parameters such as the circumferential rotation angle and pitch angle of the gimbal can be adjusted appropriately.

[0056] It should be noted that in the initial state, that is, when the circumferential rotation angle and pitch angle of the gimbal are both 0, the coordinates of the intersection point V of the gimbal's base center axis and the pitch rotation axis relative to point O are (I,J,K), which are known values, and the coordinates of the laser ranging emission point Q relative to point V are (R,T,B), which are also known values.

[0057] 2) Control the gimbal to rotate to complete the laser demolition point marking and positioning, and read the current rotation angle of the rotating platform, the circumferential rotation angle of the gimbal, and the pitch angle of the gimbal;

[0058] See Figure 2 The rotation angle of the slewing platform of the demolitionable engineering machinery is read as W, the circumferential rotation angle of the gimbal around the central axis of the gimbal base is E, and the pitch angle of the gimbal around the pitch rotation axis is P. The gimbal is defined to first rotate circumferentially and then rotate pitch.

[0059] 3) Calculate the coordinates (relative to system point O) of the laser breaking point S (Xs, Ys, Zs) after the laser beam length L is emitted by the laser rangefinder. The laser breaking point S is as follows: Figure 2 As shown, the coordinate values ​​are calculated as follows:

[0060] X S =I*cosW-J*sinW+(R*cosE-T*sinE)*cosW*cosP-(R*sinE+T*cosE)*sinW-B*cosW*sinP+L*(cosE*cosW*cosP-sinE*sinW*cosP-cosE*sinW*sinP);

[0061] Y S =I*sinW+J*cosW+(R*cosE-T*sinE)*sinW*cosP+(R*sinE+T*cosE)*cosW-B*sinW*sinP+L*(cosE*sinW*cosP+sinE*cosW*cosP+cosE*cosW*sinP);

[0062] Z S =K+B*cosP+L*sinP.

[0063] 4) After the demolition movement, determine whether the rotating platform has rotated. If so, calculate the circumferential rotation angle and pitch angle that the gimbal should maintain based on the rotation angle of the rotating platform in the previous two movements and the coordinates of the laser demolition point; otherwise, the gimbal does not need to move.

[0064] For example, when the rotation angle of the slewing platform of a demolition-resistant engineering machinery changes from W to M, if the coordinates of point S are to remain unchanged, the circumferential rotation angle E' and pitch angle P' of the gimbal are: E'=arctan2(α,β), , , P'=arcsin((Z S -KB*cosP) / L);

[0065] Where arctan2(α,β) represents the angle between the line connecting point (α,β) and the origin and the positive X-axis, the value of E' is in the range of [-180°, 180°], and the value of P' is in the range of [-90°, 90°]. When the calculated value exceeds the range, the nearest boundary value should be taken.

[0066] By assigning the calculated E' and P' values ​​to the circumferential rotation and pitch angles of the gimbal in real time, the laser breaking point can be kept constant.

[0067] Using the above-mentioned technical means, after the laser demolition point indication is completed by manipulating the gimbal rotation, once the laser indication system detects the rotation signal of the slewing platform of the demolitionable construction machinery, it automatically assigns the newly collected rotation angle value to the target following algorithm. The algorithm calculates the new circumferential rotation angle and pitch angle value of the gimbal, and then assigns them to the gimbal through the controller, driving the laser rangefinder to follow the rotation. This ensures that the current demolition laser indication point remains unchanged before the next demolition indication point is calibrated, reducing the time for the next positioning operation and improving rescue efficiency.

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

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

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

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

[0072] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the specific implementation of the present invention. Any modifications or equivalent substitutions that do not depart from the spirit and scope of the present invention should be covered within the protection scope of the claims of the present invention.

Claims

1. An automatic target-following demolition positioning system, installed on the rotating platform of demolition-capable construction machinery, characterized in that, The demolition positioning system includes: a laser rangefinder, a gimbal, an angle encoder, a controller, and a gimbal control handle. The angle encoder is used to measure the rotation angle of the rotary platform in real time and feed it back to the controller; The controller is used to calculate the circumferential rotation angle and pitch angle that the gimbal should maintain based on the rotation angle of the rotating platform and the coordinates of the laser demolition point located by the laser rangefinder, and assign the values ​​to the gimbal.

2. The automatic target-following demolition positioning system according to claim 1, characterized in that, The laser rangefinder is mounted on the gimbal, which has the functions of circumferential rotation and pitch rotation, driving the laser rangefinder to perform circumferential rotation and pitch rotation; the laser rangefinder is used to emit laser light and measure the distance between the laser point and the emission point.

3. The automatic target-following demolition positioning system according to claim 1, characterized in that, The gimbal control handle is used to operate the gimbal to rotate circumferentially and pitch, so as to drive the laser rangefinder to emit a laser beam to the laser demolition point.

4. A method for automatically tracking and positioning a demolition target point, characterized in that, Based on the automatic target-following demolition positioning system according to any one of claims 1 to 3, the method includes: Define the coordinate system and initial state of the demolition positioning system, and define the gimbal to first rotate circumferentially and then rotate in pitch. The gimbal is rotated to complete the laser demolition point marking and positioning, and the rotation angle of the current rotating platform, the circumferential rotation angle of the gimbal, and the pitch angle of the gimbal are read. Calculate the coordinates of the laser breaking point S after the laser beam length L is emitted by the laser rangefinder; After the demolition operation, if the rotating platform rotates, the circumferential rotation angle and pitch angle that the gimbal should maintain are calculated based on the rotation angle of the rotating platform in the two previous operations and the coordinates of the laser demolition point, and then assigned to the gimbal.

5. The automatic target-following demolition positioning method according to claim 4, characterized in that, The coordinate system defining the demolition positioning system includes: Let the front of the demolishable construction machinery be X+, the left side of the demolishable construction machinery be Y+, the rotation axis of the slewing platform be Z, the upward direction be Z+, and the intersection of the upper plane of the slewing bearing of the demolishable construction machinery and the rotation axis be the origin O.

6. The automatic target-following demolition positioning method according to claim 4, characterized in that, The initial state of the demolition positioning system is defined as follows: Initially, the rotation angle of the slewing platform of the demolition machinery is 0°, and the rotation angle range is -180° to +180°. From the Z+ top view, counterclockwise is positive and clockwise is negative. The position of the gimbal relative to the demolition machinery remains unchanged, the coordinate direction is consistent with the demolition positioning system, the circumferential rotation angle is 0°, and the circumferential rotation angle range is -180°~+180°. From the Z+ top view, counterclockwise is positive and clockwise is negative. The pitch angle is 0°, and the pitch angle range is -90° to +90°. From the Y+ perspective, counterclockwise is positive and clockwise is negative. The laser beam is directed at X+.

7. The automatic target-following demolition positioning method according to claim 6, characterized in that, The coordinates of the laser breaking point S after calculating the length L of the laser beam emitted by the laser rangefinder include: X S =I*cosW-J*sinW+(R*cosE-T*sinE)*cosW*cosP-(R*sinE+T*cosE)*sinW-B*cosW*sinP+L*(cosE*cosW*cosP-sinE*sinW*cosP-cosE*sinW*sinP); Y S =I*sinW+J*cosW+(R*cosE-T*sinE)*sinW*cosP+(R*sinE+T*cosE)*cosW-B*sinW*sinP+L*(cosE*sinW*cosP+sinE*cosW*cosP+cosE*cosW*sinP); Z S =K+B*cosP+L*sinP; Among them, X S Y S Z S These are the coordinates of the laser demolition point S, I, J, K, and the coordinates of the intersection point V of the gimbal's base center axis and pitch rotation axis relative to point O in the initial state. R, T, B are the coordinates of the laser ranging emission point Q relative to point V in the initial state. W is the rotation angle of the current demolitionable engineering machinery rotary platform, and E and P are the current circumferential rotation angle and pitch angle of the gimbal.

8. The automatic target-following demolition positioning method according to claim 7, characterized in that, Based on the rotation angles of the two rotating platforms and the coordinates of the laser demolition point, the circumferential rotation angle and pitch angle that the gimbal should maintain are calculated, including: E'=arctan2(α,β), α=(Y S -I*sinM-J*cosM-B*sinM*sinP-L*cosM*sinP)*cosM-( X S -I*cosM+J*sinM+B*cosM*sinP+L*sinM*sinP)*sinM, β=(X S -I*cosM+J*sinM+B*cosM*sinP+L*sinM*sinP)*cosM+(Y S -I*sinM-J*cosM-B*sinM*sinP-L*cosM*sinP)*sinM) , P'=arcsin((Z S -KB*cosP) / L); Where arctan2(α,β) represents the angle between the line connecting point (α,β) and the origin and the positive X-axis, and E' and P' are the circumferential rotation angle and pitch angle that the gimbal should maintain.

9. The automatic target-following demolition positioning method according to claim 8, characterized in that, The method also includes, If the calculated circumferential rotation angle and pitch angle that the gimbal should maintain exceed the corresponding range, then the boundary value of the nearest range shall be taken as the circumferential rotation angle and pitch angle that the gimbal should maintain.

Images