Method, processor and system for determining the distance of an engineering vehicle boom to an obstacle

Abstract

The application relates to the technical field of engineering vehicles, in particular to a method for determining the distance between the boom of an engineering vehicle and an obstacle, a processor and a system. The method comprises the following steps: establishing a first coordinate system; acquiring the first point cloud coordinates of each boom in the first coordinate system and the real-time inclination angle of each boom in real time; determining the pivot center of each boom and establishing a plurality of second coordinate systems; determining the conversion matrix between the first coordinate system and the second coordinate system of each boom according to the pivot center and the real-time inclination angle of each boom; for each boom, determining the corresponding second point cloud coordinates of each first point cloud coordinate of the boom in the second coordinate system of the boom according to the conversion matrix; determining the bounding box model of each boom according to the contour parameters of each boom and the corresponding second point cloud coordinates; and determining the distance between each boom and the obstacle according to the bounding box model and the point cloud coordinates of the obstacle. The method improves the calculation speed and accuracy of the distance between the boom of the engineering vehicle and the obstacle.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of engineering vehicles, and particularly relates to a method for determining the distance between the boom of an engineering vehicle and an obstacle, a processor, a system and a storage medium. BACKGROUND

[0002] Many engineering vehicles include rotatable booms, such as pump trucks, aerial platform vehicles, etc. During the movement of the boom, the boom may collide with obstacles such as buildings, high-voltage lines, etc., causing damage to the boom. Therefore, it is necessary to determine the distance between the boom and the obstacle during the movement of the boom.

[0003] In the prior art, the method for determining the distance is to establish the bounding box models of the boom and the obstacle, and to determine the distance between the two bounding box models to determine the distance between the boom and the obstacle. However, the obstacle is irregularly shaped, and the spherical bounding box model established for the obstacle is quite different from the actual obstacle entity, that is, the spherical bounding box has poor compactness, resulting in a large difference between the determined distance and the actual distance. If the bounding box models of the boom and the obstacle with high compactness are established at the same time, the calculation amount is too large, and it is difficult to realize real-time determination of the distance between the boom and the obstacle. SUMMARY

[0004] The purpose of the embodiments of the present application is to provide a method for determining the distance between the boom of an engineering vehicle and an obstacle, a processor, a system, an engineering vehicle and a storage medium.

[0005] In order to achieve the above-mentioned purpose, the first aspect of the present application provides a method for determining the distance between the boom of an engineering vehicle and an obstacle, the engineering vehicle including a plurality of rotatable booms, a laser radar device being installed on any one of the booms, and the method comprising:

[0006] establishing a first coordinate system, wherein the first coordinate system refers to a planar rectangular coordinate system established by the laser radar device with the position of the collection point thereof as the origin;

[0007] obtaining a plurality of first point cloud coordinates of each boom in the first coordinate system and the real-time inclination angle of each boom in real time;

[0008] determining the center of the rotation shaft of each boom according to the plurality of first point cloud coordinates of each boom and establishing a plurality of second coordinate systems, wherein the second coordinate system is a planar rectangular coordinate system established with the center of the rotation shaft of each boom as the origin, for the second coordinate system corresponding to each boom, the extension direction of the boom is the positive direction of the X-axis of the second coordinate system, the direction perpendicular to the bottom plane of the boom and upward is the positive direction of the Z-axis of the second coordinate system, and the positive direction of the Y-axis is determined based on the right-hand rule;

[0009] determining the conversion matrix between the first coordinate system and the second coordinate system of each boom according to the center of the rotation shaft of each boom and the real-time inclination angle;

[0010] For each arm support, determine, according to the conversion matrix, a second point cloud coordinate corresponding to each first point cloud coordinate of the arm support in a second coordinate system of the arm support;

[0011] Determine a bounding box model of each arm support according to the outer shape parameter of each arm support and the corresponding second point cloud coordinate;

[0012] In the case that there is an obstacle around the arm support of the engineering vehicle, obtain a plurality of third point cloud coordinates of each arm support and the obstacle in the first coordinate system;

[0013] Determine, according to the conversion matrix, a plurality of fourth point cloud coordinates corresponding to each third point cloud coordinate in each second coordinate system;

[0014] Determine the distance between each arm support and the obstacle according to the bounding box model and the plurality of fourth point cloud coordinates.

[0015] In the embodiments of the present application, determining the distance between each arm support and the obstacle according to the bounding box model and the plurality of fourth point cloud coordinates comprises: after obtaining the plurality of fourth point cloud coordinates, eliminating the point cloud coordinates in the fourth point cloud coordinates that coincide with the bounding box model to obtain a fifth point cloud coordinate of the obstacle; determining a sixth point cloud coordinate corresponding to the fifth point cloud coordinate projected in a two-dimensional ordered coordinate space; for each bounding box model, determining a two-dimensional bounding box model corresponding to the bounding box model in the two-dimensional ordered coordinate space; and determining the distance between each arm support and the obstacle according to the two-dimensional bounding box model and the sixth point cloud coordinate.

[0016] In the embodiments of the present application, determining the distance between each arm support and the obstacle according to the two-dimensional bounding box model and the sixth point cloud coordinate comprises: in the case that the distance between the position corresponding to any one of the sixth point cloud coordinates and the two-dimensional bounding box model is zero, determining that the arm support corresponding to the bounding box model collides with the obstacle; in the case that it is determined that the arm support does not collide with the obstacle, controlling the two-dimensional bounding box model to move multiple times to determine the distance between the two-dimensional bounding box model and the sixth point cloud coordinate in each direction, and further determine the distance between each arm support and the obstacle.

[0017] In the embodiment of the present application, when it is determined that the arm support does not collide with the obstacle, the two-dimensional bounding box model is controlled to move multiple times to determine the distance between the two-dimensional bounding box model and the sixth point cloud coordinates in each direction, and then the distance between each arm support and the obstacle is determined. The distance between the two-dimensional bounding box model and the sixth point cloud coordinates in the preset direction by a preset distance is determined. When the two-dimensional bounding box model does not coincide with any one of the sixth point cloud coordinates, the two-dimensional bounding box model is moved again in the preset direction by a preset distance. When the two-dimensional bounding box model coincides with any one of the sixth point cloud coordinates, the two-dimensional bounding box model is controlled to move multiple times by bisection until the two-dimensional bounding box model does not coincide with any one of the sixth point cloud coordinates, and the boundary of the two-dimensional bounding box model coincides with any one or more of the sixth point cloud coordinates. The distance between the two-dimensional bounding box model and the sixth point cloud coordinates in the preset direction is determined according to the displacement amount of the multiple movements. The distance between the two-dimensional bounding box model and the sixth point cloud coordinates in all directions is determined to determine the distance between each arm support and the obstacle.

[0018] In the embodiment of the present application, the laser radar device is installed on the first arm support, and the conversion matrix between the first coordinate system and the second coordinate system of each arm support is determined according to the rotation axis center and the real-time inclination of each arm support. The first translation amount between the rotation axis center of the first arm support and the rotation axis center of the second arm support is determined, wherein the second arm support is the arm support adjacent to the first arm support. The first translation matrix between the second coordinate systems corresponding to the first arm support and the second arm support respectively is determined according to the first translation amount. The first rotation matrix between the second coordinate systems corresponding to the first arm support and the second arm support respectively is determined according to the real-time inclination of the first arm support and the second arm support. The second rotation matrix between the second coordinate system corresponding to the first arm support and the first coordinate system is determined according to the real-time inclination of the first arm support. The first conversion matrix between the first coordinate system and the second coordinate system of the second arm support is determined according to the first translation matrix, the first rotation matrix and the second rotation matrix.

[0019] In the embodiment of the present application, the conversion matrix between the first coordinate system and the second coordinate system of each arm support is determined according to the rotation axis center and the real-time inclination of each arm support. The second translation amount between the rotation axis center of the third arm support and the rotation axis center of the last adjacent arm support is determined, wherein the last adjacent arm support is located between the third arm support and the first arm support. The second translation matrix between the second coordinate systems corresponding to the third arm support and the last adjacent arm support respectively is determined according to the second translation amount. The third rotation matrix between the second coordinate systems corresponding to the third arm support and the last adjacent arm support respectively is determined according to the real-time inclination of the third arm support and the last adjacent arm support. The second conversion matrix between the first coordinate system and the second coordinate system of the third arm support is determined according to the second translation matrix, the third rotation matrix and the conversion matrix between the last adjacent arm support and the first arm support.

[0020] In the embodiments of the present application, the laser radar device is installed on the first arm support, the rotation shaft center of each arm support is determined according to the plurality of first point cloud coordinates of each arm support, and the plurality of second coordinate systems are established, which comprises: for a target arm support other than the first arm support, the target arm support is controlled to rotate multiple times; after each rotation, the point cloud diagram corresponding to the different inclination angles of the target arm support is collected by the laser radar device; a plurality of two-dimensional point cloud diagrams of each point cloud diagram in the X-Z coordinate plane of the second coordinate system of the previous arm support are determined, wherein the previous arm support is the first arm support or an arm support between the target arm support and the first arm support; a plurality of straight line equations corresponding to the plurality of two-dimensional point cloud diagrams are determined by Hough straight line transformation; the common intersection formed by the plurality of two-dimensional point cloud diagrams is determined according to the straight line equations, so as to determine the rotation shaft center of the target arm support; the second coordinate system corresponding to the target arm support is established; the next arm support of the target arm support is controlled to rotate multiple times; and the rotation shaft center of the next arm support is determined according to the plurality of two-dimensional point cloud diagrams of the next arm support in the X-Z coordinate plane of the second coordinate system of the target arm support.

[0021] In the embodiments of the present application, the common intersection formed by the plurality of two-dimensional point cloud diagrams is determined, so as to determine the rotation shaft center of the target arm support, which comprises: determining the common intersection formed by the plurality of two-dimensional point cloud diagrams; and traversing in a preset range around the common intersection, so as to determine the rotation shaft center of the target arm support.

[0022] The second aspect of the present application provides a processor configured to execute the method for determining the distance between the arm support of the engineering vehicle and the obstacle.

[0023] The third aspect of the present application provides a system for determining the distance between the arm support of the engineering vehicle and the obstacle, which comprises: a laser radar device, configured to obtain a plurality of first point cloud coordinates of each arm support in a first coordinate system; in the case that there is an obstacle around the arm support of the engineering vehicle, a plurality of third point cloud coordinates of each arm support and the obstacle in the first coordinate system are obtained; and the above-mentioned processor.

[0024] The fourth aspect of the present application provides an engineering vehicle, which comprises: a plurality of rotatable arm supports; and the system for determining the distance between the arm support of the engineering vehicle and the obstacle.

[0025] The fifth aspect of the present application provides a machine readable storage medium, which stores instructions, and the instructions make the processor configured to execute the system for determining the distance between the arm support of the engineering vehicle and the obstacle when the instructions are executed by the processor.

[0026] By the technical solution, after the bounding box model of the boom is constructed, the processor can determine the point cloud coordinates of the obstacle in each second coordinate system according to the conversion matrix, determine the distance between the boom bounding box model and the point cloud coordinates of the obstacle in the second coordinate system, and then determine the distance between the boom and the obstacle. Compared with constructing the bounding box model for the boom and the obstacle respectively, the application constructs one less bounding box model, thereby reducing the amount of calculation. After the bounding box model is constructed, the processor can determine the conversion matrix between each second coordinate system and the first coordinate system according to the inclination angle of each boom. Further, the corresponding point cloud coordinates of the obstacle in the second coordinate system are determined according to the point cloud coordinates of the obstacle in the first coordinate system and the conversion matrix. When the boom rotates, the second coordinate system adjusts the direction of the coordinate axis in real time according to the inclination angle of the boom, and the processor can determine the point cloud coordinates of the obstacle in the second coordinate system according to the converted conversion matrix. The distance between the boom and the obstacle is determined again according to the bounding box model in the second coordinate system and the point cloud coordinates of the obstacle. The distance between the boom and the obstacle can be determined in real time during the movement of the boom. Moreover, compared with determining the distance between the boom bounding box and the spherical obstacle bounding box, the method of the application can accurately and quickly determine the distance between the boom bounding box and the point cloud coordinates of the obstacle.

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

[0028] The accompanying drawings are included to provide a further understanding of the embodiments of the application, and constitute a part of the specification, and are used to explain the embodiments of the application together with the following detailed description, but do not constitute a limitation of the embodiments of the application. In the drawings:

[0029] Figure 1 A flowchart of a method for determining the distance between the boom of the engineering vehicle and the obstacle according to the embodiments of the application is schematically shown;

[0030] Figure 2 A schematic view of a two-dimensional bounding box model and a sixth point cloud coordinate in a Y-Z two-dimensional ordered space according to the embodiments of the application is schematically shown;

[0031] Figure 3 A boom point cloud diagram according to the embodiments of the application is schematically shown;

[0032] Figure 4 A boom bounding box model diagram according to the embodiments of the application is schematically shown;

[0033] Figure 5 A boom coordinate system diagram of the engineering vehicle according to the embodiments of the application is schematically shown;

[0034] Figure 6 Fig. 1 schematically shows a structural block diagram of a system for determining the distance between the arm of an engineering vehicle and an obstacle according to an embodiment of the present application;

[0035] Figure 7 Fig. 4 schematically shows an internal structural diagram of a computer device according to an embodiment of the present application.

[0036] Reference signs

[0037] 201 Sixth point cloud coordinate set 202 Two-dimensional bounding box model

[0038] 301 Point cloud diagram of arm B 302A First point cloud diagram of arm A

[0039] 302B Second point cloud diagram of arm A 501 Laser radar device

[0040] 502 First arm 503 Second arm DETAILED DESCRIPTION

[0041] In order to make the purposes, technical solutions and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. It should be understood that the specific embodiments described herein are only used to illustrate and explain the embodiments of the present application, and are not used to limit the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative labor fall within the scope of protection of the present application.

[0042] Figure 1 Fig. 1 schematically shows a structural block diagram of a system for determining the distance between the arm of an engineering vehicle and an obstacle according to an embodiment of the present application; Figure 1 As shown in Fig. 1, in an embodiment of the present application, a method for determining the distance between the arm of an engineering vehicle and an obstacle is provided, the engineering vehicle comprising a plurality of rotatable arms, a laser radar device being installed on any one of the arms, and the method comprising the following steps:

[0043] S102, a first coordinate system is established, wherein the first coordinate system refers to a plane rectangular coordinate system established by the laser radar device with the position of the point collected by the laser radar device as the origin.

[0044] S104, a plurality of first point cloud coordinates of each arm in the first coordinate system and a real-time inclination angle of each arm are obtained in real time.

[0045] S106, determine the rotation axis center of each arm support according to the plurality of first point cloud coordinates of each arm support, and establish a plurality of second coordinate systems, wherein the second coordinate system is a plane rectangular coordinate system with the rotation axis center of each arm support as the origin, and for the second coordinate system corresponding to each arm support, the extension direction of the arm support is the positive direction of the X-axis of the second coordinate system, the direction perpendicular to the bottom plane of the arm support and upward is the positive direction of the Z-axis of the second coordinate system, and the positive direction of the Y-axis is determined based on the right-hand rule.

[0046] S08, determine the conversion matrix between the first coordinate system and the second coordinate system of each arm support according to the rotation axis center and the real-time inclination angle of each arm support.

[0047] S110, for each arm support, determine the second point cloud coordinates corresponding to each first point cloud coordinate of the arm support in the second coordinate system of the arm support according to the conversion matrix.

[0048] S112, determine the bounding box model of each arm support according to the outer shape parameters of each arm support and the corresponding second point cloud coordinates.

[0049] S114, in the case that there is an obstacle around the arm support of the engineering vehicle, obtain a plurality of third point cloud coordinates of each arm support and the obstacle in the first coordinate system.

[0050] S116, determine a plurality of fourth point cloud coordinates corresponding to each third point cloud coordinate in each second coordinate system according to the conversion matrix.

[0051] S118, determine the distance between each arm support and the obstacle according to the bounding box model and the plurality of fourth point cloud coordinates.

[0052] During the rotation of the arm support, the arm support may collide with obstacles such as power poles and buildings, so it is necessary to determine the distance between the arm support and the obstacle in real time to ensure that the arm support moves in a safe position. Therefore, the processor can establish a bounding box model of each arm support, obtain the point cloud coordinates of the obstacle, determine the distance between each bounding box model and the point cloud coordinates of the obstacle, and further determine the distance between each arm support and the obstacle.

[0053] Firstly, the processor of the engineering vehicle can establish a first coordinate system, wherein the first coordinate system refers to a plane rectangular coordinate system established by the laser radar device with the sampling point position as the origin, and the direction of the coordinate axis depends on the installation mode of the laser radar device. The processor can obtain a plurality of first point cloud coordinates of each arm support in the first coordinate system and a real-time inclination angle of each arm support. The real-time inclination angle can be determined by the signal transmitted by the inclination sensor on each arm support. The processor can determine the rotation shaft center of each arm support according to the plurality of first point cloud coordinates of each arm support. The processor can establish a plurality of second coordinate systems, wherein the second coordinate system is a plane rectangular coordinate system established with the rotation shaft center of each arm support as the origin. For the second coordinate system corresponding to each arm support, the extension direction of the arm support is the positive direction of the X-axis of the second coordinate system, the direction perpendicular to the bottom plane of the arm support and upward is the positive direction of the Z-axis of the second coordinate system, and the positive direction of the Y-axis is determined based on the right-hand rule. The extension direction of the arm support refers to the direction from the rotation shaft center of the arm support to the end of the arm support. When the arm support rotates or translates, the position and direction of each second coordinate system in space will also change. The processor can determine the conversion matrix between the first coordinate system and the second coordinate system of each arm support according to the rotation shaft center and the real-time inclination angle of each arm support. The rotation shaft center of each arm support is fixed in the second coordinate system of the previous arm support, and the processor can determine the conversion matrix between the second coordinate systems of adjacent arm supports according to the rotation shaft center and the real-time inclination angle of the arm support, and further determine the conversion matrix between the first coordinate system and the second coordinate system of each arm support.

[0054] For each arm support, the processor can determine the corresponding second point cloud coordinates of each first point cloud coordinate of the arm support in the second point cloud coordinate system of the arm support according to the conversion matrix. The processor can determine the bounding box model of each arm support according to the shape parameters of each arm support and the corresponding second point cloud coordinates. The shape parameters of the arm support can include the length, width and height of each arm support, and the bounding box model of each arm support is a three-dimensional bounding box model in the second coordinate system of each arm support. For any arm support, when the arm support rotates, the bounding box model of the arm support remains stationary in the second coordinate system of the arm support, but the coordinate axis direction of the second coordinate system of the arm support changes. In the case that there is an obstacle around the engineering vehicle, the processor can obtain a plurality of third point cloud coordinates of each arm support and the obstacle in the first coordinate system, and determine a plurality of fourth point cloud coordinates corresponding to each third point cloud coordinate in each second coordinate system according to the conversion matrix. The processor can determine the distance between each arm support and the obstacle according to the bounding box model and the plurality of fourth point cloud coordinates. The processor can exclude the arm support point cloud coordinates in the fourth point cloud coordinates, and the remaining point cloud coordinates in the second coordinate system are the obstacle. The processor can determine the distance between the bounding box model and the obstacle point cloud coordinates according to the bounding box model and the obstacle point cloud coordinates, and further determine the distance between the arm support and the obstacle.

[0055] In one specific embodiment, the engineering vehicle is a pump truck, including six rotatable booms, each boom is hingedly connected in sequence, the laser radar device is installed on the third boom of the engineering vehicle, and an inclination sensor is installed on each boom. The processor can establish a first coordinate system of the laser radar and determine a plurality of first point cloud coordinates of each boom in the first coordinate system and the inclination of each boom. The processor can establish six second coordinate systems, wherein the extension direction of the boom is the positive direction of the X axis, the direction perpendicular to the bottom plane of the boom upward is the positive direction of the Z axis, and the Y axis direction is determined based on the right-hand rule to establish a plane rectangular coordinate system. The processor can determine the conversion matrix between adjacent booms according to the center of the rotation shaft and the real-time inclination, and further determine the conversion matrix between the first coordinate system and each second coordinate system. When determining the distance between the sixth boom and the obstacle, the processor can determine the corresponding second point cloud coordinates of the first point cloud coordinates of the sixth boom in the second coordinate system of the sixth boom. The bounding box model of the sixth boom is determined according to the outer shape parameters of the sixth boom and the second point cloud coordinates, the bounding box model of the sixth boom is in the second coordinate system of the sixth boom, and the length, width and height of the bounding box model are parallel to the coordinate axes of the second coordinate system. In the case that there is an obstacle around the boom of the engineering vehicle, the processor can obtain a plurality of third point cloud coordinates of each boom and the obstacle in the first coordinate system, and determine a plurality of fourth point cloud coordinates corresponding to each third point cloud coordinate in the second coordinate system of the sixth boom according to the conversion matrix. The processor determines the distance between the sixth boom and the obstacle according to the sixth boom bounding box model and the obstacle point cloud coordinates in the plurality of fourth point cloud coordinates.

[0056] Through the above method, after the bounding box model of the boom is constructed, the processor can determine the point cloud coordinates of the obstacle corresponding to each second coordinate system according to the conversion matrix, determine the distance between the boom bounding box model and the obstacle point cloud coordinates in the second coordinate system, and further determine the distance between the boom and the obstacle. Compared with constructing bounding box models for the boom and the obstacle respectively, the application constructs one less bounding box model, thereby reducing the amount of calculation. After the bounding box model is constructed, the processor can determine the conversion matrix between each second coordinate system and the first coordinate system according to the inclination of each boom. Further, the obstacle point cloud coordinates in the second coordinate system are determined according to the point cloud coordinates of the obstacle in the first coordinate system and the conversion matrix. When the boom rotates, the second coordinate system adjusts the direction of the coordinate axis in real time according to the boom inclination, and the processor can determine the point cloud coordinates of the obstacle in the second coordinate system according to the converted conversion matrix. The distance between the boom and the obstacle is determined again according to the bounding box model and the obstacle point cloud coordinates in the second coordinate system. The distance between the boom and the obstacle can be determined in real time during the movement of the boom. Moreover, compared with determining the distance between the boom bounding box and the spherical obstacle bounding box, the application method can accurately and quickly determine the distance between the boom bounding box and the obstacle point cloud coordinates.

[0057] In one embodiment, determining the distance between each boom and an obstacle based on the bounding box model and multiple fourth point cloud coordinates includes: after acquiring multiple fourth point cloud coordinates, removing point cloud coordinates that coincide with the bounding box model to obtain the fifth point cloud coordinates of the obstacle; determining the sixth point cloud coordinates corresponding to the fifth point cloud coordinates projected into a two-dimensional ordered coordinate space; for each bounding box model, determining the two-dimensional bounding box model corresponding to the bounding box model in the two-dimensional ordered coordinate space; and determining the distance between each boom and the obstacle based on the two-dimensional bounding box model and the sixth point cloud coordinates. The processor can, after acquiring multiple fourth point cloud coordinates, remove the point cloud coordinates of the boom using a preset filtering algorithm, leaving the remaining point cloud coordinates as the fifth point cloud coordinates of the obstacle. The processor can determine the sixth point cloud coordinates corresponding to the fifth point cloud coordinates projected into a two-dimensional ordered coordinate space, and determine the two-dimensional bounding box model corresponding to each bounding box model in the two-dimensional ordered coordinate space. The two-dimensional ordered coordinate space is obtained by projecting a three-dimensional second coordinate system. The processor can determine the sixth point cloud coordinate projected from the fifth point cloud coordinates into the corresponding two-dimensional ordered space of each second coordinate system, and determine the two-dimensional bounding box model corresponding to each two-dimensional bounding box model in the two-dimensional ordered space coordinates. Based on the sixth point cloud coordinates and the two-dimensional bounding box model, the processor determines the distance between each boom and the obstacle.

[0058] For example, such as Figure 2 The diagram shown is a schematic representation of the fifth boom bounding box model and obstacles in a two-dimensional ordered space (YZ). Figure 2 The processor includes the sixth point cloud coordinate 201 and a two-dimensional bounding box model 202. When determining the distance between the fifth boom and the obstacle, the processor can obtain the fourth point cloud coordinate and remove the point cloud coordinates that coincide with the bounding box model of each boom to obtain the fifth point cloud coordinate of the obstacle. The YZ two-dimensional ordered space is a two-dimensional plane projected onto the YZ coordinate plane of the second coordinate axis corresponding to the fifth boom. The processor projects the fifth point cloud coordinate onto the YZ coordinate plane of the second coordinate system of the fifth boom to obtain the sixth point cloud coordinate 201. The processor can determine the bounding box model of the fifth boom projected onto the above YZ coordinate plane to obtain the two-dimensional coordinate box model 202. The processor can determine the distance between the fifth boom and the obstacle based on the distance between the two-dimensional bounding box model 202 and the sixth point cloud coordinate 201.

[0059] In one embodiment, determining the distance between each arm support and the obstacle according to the two-dimensional bounding box model and the sixth point cloud coordinates comprises: in the case that the distance between the position corresponding to any one of the sixth point cloud coordinates and the two-dimensional bounding box model is zero, determining that the arm support corresponding to the bounding box model collides with the obstacle; in the case that it is determined that the arm support does not collide with the obstacle, controlling the two-dimensional bounding box model to move multiple times to determine the distance between the two-dimensional bounding box model and the sixth point cloud coordinates in each direction, and further determine the distance between each arm support and the obstacle. The point cloud coordinates coinciding with the bounding box model are removed from the fifth point cloud coordinates, so that there is no point cloud coordinate coinciding with the two-dimensional bounding box model in the projected sixth point cloud coordinates. In the case that the processor determines that the distance between the position corresponding to any one of the sixth point cloud coordinates and the two-dimensional bounding box model is zero, the processor can determine that the arm support corresponding to the bounding box model collides with the obstacle. In the case that it is determined that the arm support collides with the obstacle, the processor can control the two-dimensional bounding box model to move multiple times, and further determine the distance between the two-dimensional bounding box model and the sixth point cloud coordinates in each direction, and further determine the distance between each arm support and the obstacle.

[0060] In one embodiment, in the case that it is determined that the arm support does not collide with the obstacle, controlling the two-dimensional bounding box model to move multiple times to determine the distance between the two-dimensional bounding box model and the sixth point cloud coordinates in each direction, and further determine the distance between each arm support and the obstacle comprises: controlling the two-dimensional bounding box model to move in a preset direction by a preset distance; in the case that the two-dimensional bounding box model does not coincide with any one of the sixth point cloud coordinates, moving in the preset direction by the preset distance again; in the case that the two-dimensional bounding box model coincides with any one of the sixth point cloud coordinates, controlling the two-dimensional bounding box model to move multiple times by bisection until the two-dimensional bounding box model does not coincide with any one of the sixth point cloud coordinates, and the boundary of the two-dimensional bounding box model coincides with any one or more of the sixth point cloud coordinates; determining the distance between the two-dimensional bounding box model and the sixth point cloud coordinates in the preset direction according to the displacement amount of the multiple movements; determining the distance between the two-dimensional bounding box model and the sixth point cloud coordinates in all directions to determine the distance between each arm support and the obstacle.

[0061] In a case where the processor determines that the arm support does not collide with the obstacle, the processor can control the two-dimensional bounding box model to move in the preset direction by a preset distance. In a case where the two-dimensional bounding box after moving by the preset distance does not coincide with any one of the sixth point cloud coordinates, the processor can determine that the arm support does not collide with the obstacle after moving by the preset distance. The processor can control the two-dimensional bounding box model to move by the preset distance again until the two-dimensional bounding box model coincides with any one of the sixth point cloud coordinates. In a case where the two-dimensional bounding box model coincides with any one of the sixth point cloud coordinates, the processor can control the two-dimensional bounding box model to move multiple times by the bisection method until the two-dimensional bounding box model does not coincide with any one of the sixth point cloud coordinates and the boundary of the two-dimensional bounding box model coincides with the any one or more of the sixth point cloud coordinates. The processor can determine the distance between the two-dimensional bounding box model and the sixth point cloud coordinates in the preset direction according to the displacement amount of the multiple movements, and can determine the distance between the two-dimensional bounding box model and the sixth point cloud coordinates in all directions according to the above method, and further determine the distance between each arm support and the obstacle.

[0062] For example, the obstacle is located 1.25 meters to the left of the fifth arm support, and the processor determines the distance between the fifth arm support and the obstacle. The processor can project the bounding box model corresponding to the fifth arm support on the Y-Z plane of the second coordinate system of the fifth arm support, and project the point cloud coordinates of the obstacle on the Y-Z plane of the second coordinate system of the fifth arm support to obtain the sixth point cloud coordinates. The processor can control the two-dimensional bounding box model of the fifth arm support to move to the left by a preset distance, for example, the preset distance can be the distance corresponding to the movement of the fifth arm support by 1 meter. After the second movement, the two-dimensional bounding box model of the arm support coincides with the sixth point cloud coordinates. When the processor controls the two-dimensional bounding box model to move for the third time, the processor can control the two-dimensional bounding box model to move to the right by half of the preset distance, i.e., 0.5 meters to the right. After the third movement, the total displacement amount of the two-dimensional bounding box model is 1.5 meters. After the third movement, the two-dimensional bounding box model does not coincide with any one of the sixth point cloud coordinates, and the boundary of the two-dimensional bounding box model coincides with the any one or more of the sixth point cloud coordinates. The processor can determine that the total displacement amount of the two-dimensional bounding box model after the fourth movement is the distance between the two-dimensional bounding box model and the sixth point cloud coordinates, and further determine that the distance between the obstacle to the left and the fifth arm support is 1.25 meters, which is the total displacement amount of the two-dimensional bounding box model. By using the above method, the processor determines the distance between the two-dimensional bounding box model and the sixth point cloud coordinates in all directions, and further determines the distance between the fifth arm support and the obstacle in all directions. Furthermore, the processor can determine the distance between each arm support and the obstacle.

[0063] The obstacle is usually irregular in shape, and the corresponding point cloud image is also irregular in shape. It is difficult for the processor to determine the boundary of the obstacle point cloud coordinates, and further difficult to determine the distance between the obstacle and the arm support through the image. The application determines the two-dimensional bounding box model by dichotomy multiple times, which can quickly determine the distance between the two-dimensional bounding box model and the sixth point cloud coordinates, and further determine the distance between the arm support and the obstacle.

[0064] In one embodiment, the laser radar device is installed on the first arm support, and the conversion matrix between the first coordinate system and the second coordinate system of each arm support according to the center of the rotation axis and the real-time inclination of each arm support comprises: determining a first translation between the center of the rotation axis of the first arm support and the center of the rotation axis of the second arm support, wherein the second arm support is the arm support adjacent to the first arm support; determining a first translation matrix between the second coordinate systems corresponding to the first arm support and the second arm support according to the first translation; determining a first rotation matrix between the second coordinate systems corresponding to the first arm support and the second arm support according to the real-time inclination of the first arm support and the adjacent arm support; determining a second rotation matrix between the second coordinate system corresponding to the first arm support and the first coordinate system according to the real-time inclination of the first arm support; and determining the first conversion matrix between the first coordinate system and the second coordinate system of the second arm support according to the first translation matrix, the first rotation matrix and the second rotation matrix.

[0065] After the processor determines the center of the rotation axis and the real-time inclination of each arm support, the translation matrix and the rotation matrix between each adjacent coordinate system can be determined, and further the conversion matrix between the adjacent coordinate systems can be determined. When the laser radar is installed on the first arm support, the processor can determine a first translation between the center of the rotation axis of the first arm support and the center of the rotation axis of the second arm support, and according to the first translation, the processor can determine a first translation matrix between the second coordinate systems corresponding to the first arm support and the second arm support. According to the real-time inclination of the first arm support and the second arm support, a first rotation matrix between the second coordinate systems corresponding to the first arm support and the second arm support can be determined. According to the real-time inclination of the first arm support, a second rotation matrix between the second coordinate system corresponding to the first arm support and the first coordinate system can be determined. The first coordinate system takes the position of the point collected by the laser radar device as the origin, and the position of the point collected by the laser radar can be placed at the center of the rotation axis of the first arm support. Therefore, the processor can determine the conversion matrix between the second coordinate system corresponding to the first arm support and the second coordinate system corresponding to the second arm support according to the first translation matrix and the first rotation matrix. Further, according to the second rotation matrix and the conversion matrix between the second coordinate system corresponding to the first arm support and the second coordinate system corresponding to the second arm support determined above, the conversion matrix between the first coordinate system and the second coordinate system corresponding to the second arm support is determined.

[0066] In one embodiment, the collection point position of the laser radar is placed at the center of the rotation shaft of the first arm support. The conversion matrix between the first coordinate system and the second coordinate system of the first arm support is a first rotation matrix.

[0067] In one embodiment, the conversion matrix between the first coordinate system and the second coordinate system of each arm support according to the center of the rotation shaft and the real-time tilt angle of each arm support further comprises: determining the translation amount between the center of the rotation shaft of the third arm support and the center of the rotation shaft of the last adjacent arm support, wherein the last adjacent arm support is located between the third arm support and the first arm support; determining the second translation matrix between the second coordinate systems corresponding to the third arm support and the last adjacent arm support respectively according to the translation amount; determining the third rotation matrix between the second coordinate systems corresponding to the third arm support and the last adjacent arm support respectively according to the real-time tilt angles of the third arm support and the last adjacent arm support; and determining the second conversion matrix between the first coordinate system and the second coordinate system of the third arm support according to the second translation matrix, the third rotation matrix, and the conversion matrix between the last adjacent arm support and the first arm support.

[0068] The processor can determine the second translation amount between the center of the rotation shaft of the third arm support and the last adjacent arm support, and according to the second translation amount, the processor can determine the second translation matrix between the second coordinate systems corresponding to the third arm support and the last adjacent arm support. According to the real-time tilt angles between the third arm support and the last adjacent arm support, the third rotation matrix between the second coordinate systems corresponding to the third arm support and the last adjacent arm support can be determined. The processor can determine the second conversion matrix between the first coordinate system and the second coordinate system of the third arm support according to the second translation matrix, the third rotation matrix, and the conversion matrix between the last adjacent arm support and the first arm support.

[0069] In one specific embodiment, the laser radar is installed on the first section of the arm support, and the collection point of the laser radar is located at the center of the rotation shaft of the first section of the arm support. The processor determines the fourth rotation matrix between the second coordinate system corresponding to the first section of the arm support and the first coordinate system according to the real-time inclination of the first section of the arm support. The fourth rotation matrix is the third conversion matrix between the second coordinate system corresponding to the first section of the arm support and the first coordinate system. The processor can determine the third translation matrix between the second section of the arm support and the first section of the arm support according to the third translation amount between the center of the rotation shaft of the second section of the arm support and the center of the rotation shaft of the first section of the arm support, and determine the fifth rotation matrix between the second coordinate system corresponding to the first section of the arm support and the second coordinate system corresponding to the second section of the arm support according to the real-time inclinations of the first section of the arm support and the second section of the arm support. The processor can determine the fourth rotation matrix between the first section of the arm support and the second section of the arm support according to the third translation matrix and the fifth rotation matrix, and determine the fifth conversion matrix between the second coordinate system corresponding to the second section of the arm support and the first coordinate system according to the third conversion matrix and the fourth conversion matrix. Similarly, the fourth translation matrix can be determined according to the fourth translation amount between the center of the rotation shaft of the third section of the arm support and the center of the rotation shaft of the second section of the arm support, and the sixth rotation matrix can be determined according to the real-time inclinations of the third section of the arm support and the second section of the arm support. The processor can determine the sixth conversion matrix between the second coordinate system corresponding to the second section of the arm support and the first coordinate system according to the fourth translation matrix, the sixth rotation matrix and the fifth conversion matrix. The processor can determine the conversion matrix between the second coordinate system corresponding to each section of the arm support and the first coordinate system according to the method of determining the conversion matrix, the translation matrix and the rotation matrix between the adjacent arm supports section by section.

[0070] In one embodiment, the laser radar device is installed on the first arm support, and determining the rotation shaft center of each arm support and establishing a plurality of second coordinate systems according to a plurality of first point cloud coordinates of each arm support comprises: for a target arm support other than the first arm support, controlling the target arm support to rotate multiple times; after each rotation, collecting a point cloud diagram corresponding to a different inclination of the target arm support by the laser radar device; determining a plurality of two-dimensional point cloud diagrams of each point cloud diagram in the X-Z coordinate plane of the second coordinate system of the previous arm support, wherein the previous arm support is the first arm support or an arm support located between the target arm support and the first arm support; determining a plurality of straight line equations corresponding to the plurality of two-dimensional point cloud diagrams by Hough straight line transformation; determining a common intersection formed by the plurality of two-dimensional point cloud diagrams according to the straight line equations to determine the rotation shaft center of the target arm support; establishing the second coordinate system corresponding to the target arm support; controlling the next arm support of the target arm support to rotate multiple times; and determining the rotation shaft center of the next arm support according to a plurality of two-dimensional point cloud diagrams of the next arm support in the X-Z coordinate plane of the second coordinate system of the target arm support.

[0071] When determining the center position of each boom pivot, the processor can perform multiple rotations on the target boom (excluding the first boom), acquiring a point cloud map of the target boom after each rotation. The boom of an engineering vehicle is typically on a plane, and each boom rotates around its own Y-axis in its second coordinate system during rotation. Multiple two-dimensional point cloud maps of each boom in the XZ coordinate plane of the previous boom's second coordinate system are determined. The processor can use the Hough line transform to determine multiple linear equations corresponding to these two-dimensional point cloud maps. The Hough line transform is a feature extraction method; the processor can use it to determine the linear equations formed by the bottom of the boom. The processor can then determine the common intersection point formed by the multiple two-dimensional point cloud maps based on these linear equations, and use this intersection point as the center of rotation of the target boom. After determining the center of rotation of the target boom, a second coordinate system corresponding to the target boom can be established, and the next boom can be rotated multiple times. The center of rotation of the next boom is determined based on multiple second point cloud maps of the next boom in the XZ coordinate plane of the second coordinate system corresponding to the target boom. The offset between adjacent pivot centers does not change with boom rotation. Using the method described above, the position of each boom pivot center in the second coordinate system corresponding to the previous boom can be determined, thus enabling the processor to determine the offset between adjacent pivot centers. Because the pivot center of an engineering vehicle's boom may have significant errors compared to mechanical drawings due to installation and usage processes, the method of this application can accurately determine the pivot center of each boom.

[0072] In a specific embodiment, such as Figure 3 The diagram shows the point cloud image after the processor controls the rotation of boom A. This point cloud image is projected onto the XZ coordinate plane of the second coordinate system of the previous boom B. It includes point cloud image 301 of boom B, a first point cloud image 302A of boom A, and a second point cloud image 302B of boom A. The first point cloud image 302A represents the point cloud image of boom A in its first position, and the second point cloud image 302B represents the point cloud image of boom A in its second position. The processor can determine the angle formed between the first point cloud image 302A and the second point cloud image 302B, with the endpoints of this angle being the common intersection point, which is determined as the rotation axis center of boom A.

[0073] In one embodiment, the common intersection point formed by the plurality of two-dimensional point cloud maps is determined to determine the rotation axis center of the target arm support, comprising: determining the common intersection point formed by the plurality of two-dimensional point cloud maps; and traversing the points in a preset range near the common intersection point to determine the rotation axis center of the target arm support. Because the rotation axis center determined by the common intersection point still has a large deviation in actual calculation, the processor can traverse the points in a preset range near the common intersection point to determine the point with the smallest deviation as the rotation axis center of the target arm support. The rotation axis center determined by this method is more accurate than the common intersection point, and only the points in the preset range are traversed, which reduces the calculation amount of the processor and improves the speed of determining the rotation axis center.

[0074] In one embodiment, when the processor traverses the points in a preset range near the common intersection point, the target point P i is taken as the preset rotation axis center of the target arm support, and a preset second coordinate system of the target arm support is established, the target arm support is controlled to rotate multiple times, and the corresponding Z-axis minimum value of the target point cloud map after each rotation in the preset second coordinate system of the target arm support is determined. The Z-axis minimum value set Z min after multiple rotations is determined. min The maximum and minimum values of the minimum value set Z i are determined. The difference value corresponding to each point in the preset range is determined, and a difference value set Z i is determined. The point corresponding to the minimum value in the difference value set Z i is determined as the rotation axis center of the target arm support. Because when the selected preset rotation axis center is the accurate value, the positions of the corresponding point cloud maps after multiple rotations are the same in the target arm support coordinate system, and the deviation of the Z-axis minimum value can be ignored; when the selected preset rotation axis center has a large deviation from the actual rotation axis center, the positions of the corresponding point cloud maps after multiple rotations are offset in the target arm support coordinate system, and the deviation of the Z-axis minimum value is large.

[0075] In one specific embodiment, as shown in Figure 4 , a schematic diagram of an arm support bounding box model is shown, and the dashed line in the figure represents the established arm support bounding box model. The processor can determine the distance between the arm support bounding box model and the obstacle point cloud coordinates, and then determine the distance between the arm support and the obstacle. The size of the arm support bounding box model depends on the shape parameters of the arm support, so the arm support bounding box model used in the present application has high compactness. By determining the distance between the arm support bounding box model and the obstacle point cloud map, and then determining the distance between the arm support and the obstacle, the distance between the arm support and the obstacle is more accurate than the prior art, which determines the distance between the arm support and the obstacle by determining the distance between the arm support bounding box and the spherical obstacle bounding box.

[0076] In one specific embodiment, as shown in Figure 5The diagram shows a coordinate system for the boom of an engineering vehicle, including a lidar device 501, a first boom 502, and a second boom 503. The coordinate system corresponding to X1-Z1 is the first coordinate system, the coordinate system corresponding to X2-Z2 is the second coordinate system corresponding to the first boom 502, and the coordinate system corresponding to X3-Z3 is the second coordinate system corresponding to the second boom 503.

[0077] The above method reduces the computational load on the processor when calculating the distance between the boom and obstacles, speeding up the calculation and enabling real-time determination of the distance between the boom and obstacles during boom movement. Furthermore, compared to determining the distance between the boom bounding box and the spherical obstacle bounding box, this application determines the distance based on the obstacle's point cloud coordinates, resulting in a more accurate distance. Secondly, when determining the distance between the boom bounding box model and the obstacle, the processor divides the calculation of distances between multiple boom bounding box models and obstacles, calculating the distance between each model individually. Coordinate transformation simplifies the computational load and speeds up the calculation process, enabling the engineering vehicle processor to perform real-time calculations when determining the distance between the boom and obstacles. This application uses a bisection method to determine the distance between the two-dimensional bounding box model after multiple movements, quickly determining the distance between the two-dimensional bounding box model and the sixth point cloud coordinates, thereby determining the distance between the boom and the obstacle.

[0078] Figure 1 This is a flowchart illustrating a method for determining the distance between the boom of an engineering vehicle and an obstacle in one embodiment. It should be understood that, although... Figure 1 The steps in the flowchart are shown sequentially as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order in which these steps are executed, and they can be performed in other orders. Figure 1 At least some of the steps in the process may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least some of the sub-steps or stages of other steps.

[0079] In one embodiment, such as Figure 6 As shown, a system 600 for locating the boom of an engineering vehicle from an obstacle is provided, including a lidar device 601 and a processor 602, wherein:

[0080] The laser radar device 601 is configured to acquire a plurality of first point cloud coordinates of each arm support in the first coordinate system; and acquire a plurality of third point cloud coordinates of each arm support and the obstacle in the first coordinate system in the case that the obstacle exists around the arm support of the engineering vehicle.

[0081] The processor 602 is configured to execute the method for determining the distance between the arm support of the engineering vehicle and the obstacle.

[0082] In an embodiment, an engineering vehicle is provided, which comprises a plurality of rotatable arm supports, and the system for determining the distance between the arm support of the engineering vehicle and the obstacle.

[0083] The processor comprises a core, and the core is configured to call corresponding program units in the memory. The core can be one or more, and the core parameters are adjusted to implement the method for determining the distance between the arm support of the engineering vehicle and the obstacle.

[0084] The memory can comprise a non-permanent memory in a computer readable medium, such as a random access memory (RAM) and / or a non-volatile memory such as a read-only memory (ROM) or a flash memory (flash RAM), and the memory comprises at least one memory chip.

[0085] An embodiment of the present application provides a storage medium, which stores a program, and the program is executed by a processor to implement the method for determining the distance between the arm support of the engineering vehicle and the obstacle.

[0086] An embodiment of the present application provides a processor, which is used to run a program, and the program is executed to implement the method for determining the distance between the arm support of the engineering vehicle and the obstacle.

[0087] In an embodiment, a computer device is provided, which can be a server, and an internal structure diagram of the computer device can be as shown in FIG. 1. Figure 7 The computer device comprises a processor A01, a network interface A02, a memory (not shown in the figure) and a database (not shown in the figure) connected through a system bus. The processor A01 of the computer device is configured to provide computing and control capabilities. The memory of the computer device comprises an internal memory A03 and a non-volatile storage medium A04. The non-volatile storage medium A04 stores an operating system B01, a computer program B02 and a database (not shown in the figure). The internal memory A03 provides an environment for the operating system B01 and the computer program B02 in the non-volatile storage medium A04. The network interface A02 of the computer device is configured to communicate with an external terminal through a network connection. The computer program B02 is executed by the processor A01 to implement a method for determining the distance between the arm support of the engineering vehicle and the obstacle.

[0088] Those skilled in the art can understand that, Figure 7The structure shown in the figure is only a block diagram of part of the structure related to the scheme of the present application, and does not constitute a limitation on the computer device to which the scheme of the present application is applied. A specific computer device can include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0089] An embodiment of the present application provides a device, which comprises a processor, a memory, and a program stored on the memory and executable on the processor. When the processor executes the program, the following steps are implemented: establishing a first coordinate system, wherein the first coordinate system refers to a plane rectangular coordinate system established by a laser radar device with a position of a collection point as an origin; obtaining a plurality of first point cloud coordinates of each arm support in the first coordinate system and a real-time inclination angle of each arm support in real time; determining a rotation shaft center of each arm support according to the plurality of first point cloud coordinates of each arm support and establishing a plurality of second coordinate systems, wherein the second coordinate system is a plane rectangular coordinate system established with the rotation shaft center of each arm support as an origin, for each second coordinate system corresponding to the arm support, an extension direction of the arm support is a positive direction of an X-axis of the second coordinate system, a direction perpendicular to a bottom plane of the arm support and upward is a positive direction of a Z-axis of the second coordinate system, and a positive direction of a Y-axis is determined based on a right-hand rule; determining a conversion matrix between the first coordinate system and the second coordinate system of each arm support according to the rotation shaft center and the real-time inclination angle of each arm support; for each arm support, determining a second point cloud coordinate corresponding to each first point cloud coordinate of the arm support in the second coordinate system of the arm support according to the conversion matrix; determining a bounding box model of each arm support according to an external shape parameter of each arm support and the corresponding second point cloud coordinate; in a case where there is an obstacle around the arm support of the engineering vehicle, obtaining a plurality of third point cloud coordinates of each arm support and the obstacle in the first coordinate system; determining a plurality of fourth point cloud coordinates corresponding to each third point cloud coordinate in each second coordinate system according to the conversion matrix; and determining a distance between each arm support and the obstacle according to the bounding box model and the plurality of fourth point cloud coordinates.

[0090] In one embodiment, determining the distance between each arm support and the obstacle according to the bounding box model and the plurality of fourth point cloud coordinates comprises: after obtaining the plurality of fourth point cloud coordinates, eliminating point cloud coordinates in the fourth point cloud coordinates that coincide with the bounding box model to obtain a fifth point cloud coordinate of the obstacle; determining a sixth point cloud coordinate corresponding to the fifth point cloud coordinate projected in a two-dimensional ordered coordinate space; for each bounding box model, determining a two-dimensional bounding box model corresponding to the bounding box model in the two-dimensional ordered coordinate space; and determining the distance between each arm support and the obstacle according to the two-dimensional bounding box model and the sixth point cloud coordinate.

[0091] In one embodiment, determining the distance between each arm support and the obstacle according to the two-dimensional bounding box model and the sixth point cloud coordinates comprises: in the case that the distance between the position corresponding to any one of the sixth point cloud coordinates and the two-dimensional bounding box model is zero, determining that the arm support corresponding to the bounding box model collides with the obstacle; in the case that it is determined that the arm support does not collide with the obstacle, controlling the two-dimensional bounding box model to move multiple times to determine the distance between the two-dimensional bounding box model and the sixth point cloud coordinates in each direction, and further determine the distance between each arm support and the obstacle.

[0092] In one embodiment, in the case that it is determined that the arm support does not collide with the obstacle, controlling the two-dimensional bounding box model to move multiple times to determine the distance between the two-dimensional bounding box model and the sixth point cloud coordinates in each direction, and further determine the distance between each arm support and the obstacle comprises: controlling the two-dimensional bounding box model to move in a preset direction by a preset distance; in the case that the two-dimensional bounding box model does not coincide with any one of the sixth point cloud coordinates, moving in the preset direction by the preset distance again; in the case that the two-dimensional bounding box model coincides with any one of the sixth point cloud coordinates, controlling the two-dimensional bounding box model to move multiple times by bisection until the inside of the two-dimensional bounding box model does not coincide with any one of the sixth point cloud coordinates, and the boundary of the two-dimensional bounding box model coincides with any one or more of the sixth point cloud coordinates; determining the distance between the two-dimensional bounding box model and the sixth point cloud coordinates in the preset direction according to the displacement amount of the multiple movements; determining the distance between the two-dimensional bounding box model and the sixth point cloud coordinates in all directions to determine the distance between each arm support and the obstacle.

[0093] In one embodiment, determining the conversion matrix between the first coordinate system and the second coordinate system of each arm support according to the rotation axis center and the real-time inclination angle of each arm support comprises: determining a first translation amount between the rotation axis center of the first arm support and the rotation axis center of the second arm support, wherein the second arm support is the arm support adjacent to the first arm support; determining a first translation matrix between the second coordinate systems corresponding to the first arm support and the second arm support respectively according to the first translation amount; determining a first rotation matrix between the second coordinate systems corresponding to the first arm support and the second arm support respectively according to the real-time inclination angles of the first arm support and the second arm support; determining a second rotation matrix between the second coordinate system corresponding to the first arm support and the first coordinate system according to the real-time inclination angle of the first arm support; and determining a first conversion matrix between the first coordinate system and the second coordinate system of the second arm support according to the first translation matrix, the first rotation matrix and the second rotation matrix.

[0094] In an embodiment, determining the transformation matrix between the first coordinate system and the second coordinate system of each of the arm frames according to the rotation center of each of the arm frames and the real-time inclination angle further comprises: determining a second translation between the rotation center of the third arm frame and the rotation center of a last adjacent arm frame, wherein the last adjacent arm frame is between the third arm frame and the first arm frame; determining a second translation matrix between the second coordinate systems corresponding to the third arm frame and the last adjacent arm frame respectively according to the second translation; determining a third rotation matrix between the second coordinate systems corresponding to the third arm frame and the last adjacent arm frame respectively according to the real-time inclination angles of the third arm frame and the last adjacent arm frame; and determining a second transformation matrix between the first coordinate system and the second coordinate system of the third arm frame according to the second translation matrix, the third rotation matrix, and the transformation matrix between the last adjacent arm frame and the first arm frame.

[0095] In an embodiment, determining the rotation center of each of the arm frames and establishing the plurality of second coordinate systems according to the plurality of first point cloud coordinates of each of the arm frames comprises: for a target arm frame other than the first arm frame, controlling the target arm frame to rotate multiple times; after each rotation, collecting a point cloud map corresponding to a different inclination angle of the target arm frame by the laser radar device; determining a plurality of two-dimensional point cloud maps of each point cloud map in an X-Z coordinate plane of a second coordinate system of a last arm frame, wherein the last arm frame is the first arm frame or an arm frame between the target arm frame and the first arm frame; determining a plurality of straight line equations corresponding to the plurality of two-dimensional point cloud maps by Hough line transformation; determining a common intersection formed by the plurality of two-dimensional point cloud maps as the rotation center of the target arm frame; establishing a second coordinate system corresponding to the target arm frame; controlling a next arm frame of the target arm frame to rotate multiple times; and determining the rotation center of the next arm frame according to a plurality of two-dimensional point cloud maps of the next arm frame in an X-Z coordinate plane of the second coordinate system corresponding to the target arm frame.

[0096] In an embodiment, determining the common intersection formed by the plurality of two-dimensional point cloud maps as the rotation center of the target arm frame comprises: determining the common intersection formed by the plurality of two-dimensional point cloud maps; and traversing a preset range around the common intersection to determine the rotation center of the target arm frame.

[0097] Those skilled in the art will understand that embodiments of the present application can be provided as methods, systems, or computer program products. Therefore, the present application can take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can take the form of a computer program product implemented on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROMs, optical storage devices, etc.) having computer-usable program code embodied therein.

Claims

1. A method for determining the distance between the boom of an engineering vehicle and an obstacle, characterized in that, The engineering vehicle includes multiple rotatable booms, and a lidar device is mounted on any one of the booms. The method includes: Establish a first coordinate system, wherein the first coordinate system refers to the plane rectangular coordinate system established by the lidar device with its acquisition point position as the origin; Real-time acquisition of the cloud coordinates of multiple first points of each boom in the first coordinate system and the real-time tilt angle of each boom; The rotation axis center of each boom is determined based on multiple first point cloud coordinates of each boom, and multiple second coordinate systems are established. The second coordinate system is a plane rectangular coordinate system with the rotation axis center of each boom as the origin. For each boom, the extension direction of the boom is the positive direction of the X-axis of the second coordinate system, the direction perpendicular to the bottom plane of the boom is the positive direction of the Z-axis of the second coordinate system, and the positive direction of the Y-axis is determined based on the right-hand rule. The transformation matrix between the first coordinate system and the second coordinate system of each boom is determined based on the rotation axis center of each boom and the real-time tilt angle. For each boom, the second point cloud coordinates corresponding to each first point cloud coordinate of the boom in the second coordinate system of the boom are determined according to the transformation matrix; The bounding box model of each boom is determined based on the shape parameters of each boom and its corresponding second point cloud coordinates. In the case of obstacles around the boom of the engineering vehicle, obtain the cloud coordinates of multiple third points of each boom and the obstacles in the first coordinate system; Based on the transformation matrix, determine the multiple fourth point cloud coordinates corresponding to each third point cloud coordinate in each second coordinate system; The distance between each boom and the obstacle is determined based on the bounding box model and the multiple fourth point cloud coordinates.

2. The method for determining the distance between the boom of an engineering vehicle and an obstacle according to claim 1, characterized in that, The step of determining the distance between each boom and the obstacle based on the bounding box model and the multiple fourth point cloud coordinates includes: After obtaining the multiple fourth point cloud coordinates, the point cloud coordinates that coincide with the bounding box model are removed from the fourth point cloud coordinates to obtain the fifth point cloud coordinates of the obstacle. Determine the cloud coordinates of the sixth point corresponding to the projection of the cloud coordinates of the fifth point onto the two-dimensional ordered coordinate space; For each bounding box model, determine the corresponding two-dimensional bounding box model in the two-dimensional ordered coordinate space; The distance between each boom and the obstacle is determined based on the two-dimensional bounding box model and the sixth point cloud coordinates.

3. The method for determining the distance between the boom of an engineering vehicle and an obstacle according to claim 2, characterized in that, The step of determining the distance between each boom and the obstacle based on the two-dimensional bounding box model and the sixth point cloud coordinates includes: If the distance between the position corresponding to any sixth point cloud coordinate and the two-dimensional bounding box model is zero, it is determined that the boom corresponding to the bounding box model has collided with the obstacle. If it is determined that the boom does not collide with the obstacle, the two-dimensional bounding box model is controlled to move multiple times to determine the distance between the two-dimensional bounding box model and the sixth point cloud coordinate in each direction, and then the distance between each boom and the obstacle is determined.

4. The method for determining the distance between the boom of an engineering vehicle and an obstacle according to claim 3, characterized in that, The step of controlling the two-dimensional bounding box model to move multiple times, after determining that the boom does not collide with the obstacle, to determine the distance between the two-dimensional bounding box model and the sixth point cloud coordinates in each direction, and thus determining the distance between each boom and the obstacle, includes: Control the two-dimensional bounding box model to move a preset distance in a preset direction; If the two-dimensional bounding box model does not coincide with any of the sixth point cloud coordinates, move the preset distance again in the preset direction; When the two-dimensional bounding box model coincides with any sixth point cloud coordinate, the two-dimensional bounding box model is moved multiple times by the bisection method until the interior of the two-dimensional bounding box model does not coincide with any sixth point cloud coordinate, and the boundary of the two-dimensional bounding box model coincides with any one or more sixth point cloud coordinates. The distance between the two-dimensional bounding box model and the sixth point cloud coordinates in the preset direction is determined based on the displacement of multiple movements; Determine the distances between the two-dimensional bounding box model and the sixth point cloud coordinates in all directions to determine the distance between each boom and the obstacle.

5. The method for determining the distance between the boom of an engineering vehicle and an obstacle according to claim 1, characterized in that, The lidar device is mounted on the first boom, and determining the transformation matrix between the first coordinate system and the second coordinate system of each boom based on the rotation axis center of each boom and the real-time tilt angle includes: Determine a first translation amount between the rotation axis center of the first boom and the rotation axis center of the second boom, wherein the second boom is the boom adjacent to the first boom; Determine the first translation matrix between the second coordinate systems corresponding to the first boom and the second boom, respectively, based on the first translation amount; The first rotation matrix between the second coordinate systems corresponding to the first boom and the second boom is determined based on the real-time tilt angle between the first boom and the adjacent boom. The second rotation matrix between the second coordinate system corresponding to the first boom and the first coordinate system is determined based on the real-time tilt angle of the first boom. The first transformation matrix between the first coordinate system and the second coordinate system of the second boom is determined based on the first translation matrix, the first rotation matrix, and the second rotation matrix.

6. The method for determining the distance between the boom of an engineering vehicle and an obstacle according to claim 5, characterized in that, The step of determining the transformation matrix between the first coordinate system and the second coordinate system of each boom based on the rotation axis center of each boom and the real-time tilt angle further includes: Determine a second translation amount between the rotation axis center of the third boom and the rotation axis center of the previous adjacent boom, wherein the previous adjacent boom is located between the third boom and the first boom; Determine the second translation matrix between the second coordinate systems corresponding to the third boom and the previous adjacent boom, respectively, based on the second translation amount; The third rotation matrix between the second coordinate systems corresponding to the third boom and the previous adjacent boom is determined based on the real-time tilt angle between the third boom and the previous adjacent boom. The second transformation matrix between the first coordinate system and the second coordinate system of the third boom is determined based on the second translation matrix, the third rotation matrix, and the transformation matrix between the previous adjacent boom and the first boom.

7. The method for determining the distance between the boom of an engineering vehicle and an obstacle according to claim 1, characterized in that, The lidar device is mounted on the first boom, and the process of determining the rotation axis center of each boom and establishing multiple second coordinate systems based on multiple first point cloud coordinates of each boom includes: For a target boom other than the first boom, control the target boom to rotate multiple times; After each rotation, the lidar device collects point cloud images corresponding to different tilt angles of the target boom; Determine multiple two-dimensional point cloud maps of each point cloud map in the XZ coordinate plane of the second coordinate system of the previous boom, wherein the previous boom is the first boom or the boom located between the target boom and the first boom; The Hough linear transformation is used to determine the equations of multiple straight lines corresponding to the multiple two-dimensional point cloud maps. The common intersection point formed by the multiple two-dimensional point cloud maps is determined according to the linear equation, and is thus determined as the rotation axis center of the target boom; Establish a second coordinate system corresponding to the target boom; Control the next boom of the target boom to rotate multiple times; The rotation axis center of the next boom is determined based on multiple two-dimensional point cloud diagrams of the XZ coordinate plane of the second coordinate system corresponding to the target boom.

8. The method for determining the distance between the boom of an engineering vehicle and an obstacle according to claim 7, characterized in that, The step of determining the common intersection point formed by the plurality of two-dimensional point cloud maps, and using it as the rotation axis center of the target boom, includes: Determine the common intersection point formed by the multiple two-dimensional point cloud maps; The rotation center of the target boom is determined by traversing a preset range near the common intersection point.

9. A processor, characterized in that, Configured to perform the method for determining the distance between the boom of an engineering vehicle and an obstacle as described in any one of claims 1 to 8.

10. A system for determining the distance between the boom of an engineering vehicle and an obstacle, characterized in that, include: A lidar device is used to acquire multiple first point cloud coordinates of each boom in a first coordinate system; and, in the case of obstacles around the boom of the engineering vehicle, to acquire multiple third point cloud coordinates of each boom and the obstacles in the first coordinate system; and The processor according to claim 9.

11. An engineering vehicle, characterized in that, include: Multiple rotatable booms; as well as The system for determining the distance between the boom of an engineering vehicle and an obstacle as described in claim 10.

12. A machine-readable storage medium storing instructions thereon, characterized in that, When executed by a processor, the instruction causes the processor to be configured to perform the method for determining the distance between the boom of an engineering vehicle and an obstacle according to any one of claims 1 to 8.

Images