Cabin excavator anti-collision positioning method and system based on three-dimensional point cloud

Abstract

The invention discloses an anti-collision positioning method and system for an excavator in a cabin based on a three-dimensional point cloud, and belongs to the field of excavator positioning.The method comprises the steps that three-dimensional point cloud data and a depth map are obtained through a laser radar; sequentially carrying out point cloud segmentation and feature extraction on the three-dimensional point cloud data, and carrying out point cloud registration by combining the three-dimensional point cloud data of different frames; screening out non-cabin wall surface points of which the smoothness does not reach the standard to obtain a cabin wall depth map in the cabin; constructing an excavator model, and calculating the distance between the bucket and the bulkhead based on the excavator model and the bulkhead depth map; and a bucket collision experiment is conducted, the rotation angle limit value of the rotation structure and the joint angle limit value of the mechanical arm under different intervals are obtained, the interval difference is calculated, a virtual point cloud set is constructed, and anti-collision constraint is established with the virtual point cloud set as the reference. According to the method, the positioning information of the excavator is obtained, the movement of the mechanical arm and the rotation structure is limited through the virtual point cloud set, and collision prevention of the whole excavator is effectively guaranteed.

Description

Technical Field

[0001] The present invention relates to the technical field of positioning an excavator in a ship cabin, and in particular to a collision avoidance positioning method and system for an excavator in a ship cabin based on a three-dimensional point cloud. Background Art

[0002] The holds loaded with dry bulk cargoes are mostly cleared and transported by means of gantry cranes, loaders and excavators. The materials are loaded in the hold, which is a polygonal closed environment with an open top and bulkheads all around. A ship unloader is installed on the top of the hold. The excavator is responsible for gathering materials far away from the ship unloader to the working area of ​​the ship unloader, and the ship unloader grabs the materials and transfers them to the outside of the hold.

[0003] In order to precisely control the work of the excavator, the excavator needs to sense environmental information when operating in the closed environment of the cabin. In the prior art, lidar is usually used to send lasers around the cabin, obtain laser information reflected by the cabin wall, sense the position of the excavator in the cabin, and send the excavator's position information to the control end and share it with the ship unloader and loader. It can collide with the ship unloader, loader and other excavators entering the same area at the same time.

[0004] The existing excavator itself has a rotating structure and a mechanical arm. The rotating structure can drive the bucket to transport materials back and forth through the rotation of the mechanical arm. The rotation of the mechanical arm itself can adjust the position of the bucket to achieve loading and unloading. The position of the rotating structure and the mechanical arm is uncertain and it is impossible to accurately control the position of the rotating structure and the mechanical arm, which can easily cause the mechanical arm and bucket to collide with the bulkhead when the excavator approaches the bulkhead. Therefore, simply obtaining and controlling the position information of the excavator cannot guarantee that the collision of the excavator with the upper structure will be avoided, and the safety of the mechanical operation cannot be guaranteed. Summary of the invention

[0005] To this end, the present invention provides an anti-collision positioning method and system for an excavator in a cabin based on a three-dimensional point cloud, which effectively solves the technical problem in the prior art that only by obtaining and controlling the position information of the excavator can it be guaranteed that the collision with the structure on the excavator cannot be avoided, and the safety of the mechanical operation cannot be guaranteed.

[0006] In order to solve the above technical problems, the present invention specifically provides the following technical solutions: a method for anti-collision positioning of an excavator in a cabin based on a three-dimensional point cloud, comprising the following steps:

[0007] The laser radar is used to scan the cabin environment to obtain three-dimensional point cloud data and map it onto a two-dimensional image plane to form a depth map;

[0008] The 3D point cloud data is segmented and features extracted in sequence, and the 3D point cloud data of different frames are combined for point cloud registration to output positioning information;

[0009] Each pixel point and its adjacent pixel points in the depth map form a point unit and perform feature extraction, and filter out non-cabin wall points that do not meet the smoothness standard to obtain the depth map of the bulkhead in the cabin;

[0010] Obtain the spatial parameters of the rotary structure and mechanical arm on the excavator to build an excavator model, and calculate the distance between the bucket and the bulkhead based on the excavator model and the bulkhead depth map;

[0011] A bucket collision experiment is carried out to obtain the maximum and minimum spacing between the bucket and the bulkhead, as well as the rotation angle limit values ​​of the rotating structure and the joint angle limit values ​​of the robotic arm at different spacings, and the spacing difference is calculated. The spacing difference is used as the displacement vector to construct a virtual point cloud set. Based on the virtual point cloud set, anti-collision constraints are established according to the rotation angle limit values ​​of the rotating structure and the joint angle limit values ​​of the robotic arm at different spacings.

[0012] Further,

[0013] Point cloud segmentation and feature extraction are performed on the three-dimensional point cloud data in sequence, including the following steps:

[0014] Divide the depth map into several sub-images in the horizontal direction;

[0015] For the point cloud P at time t t = {P1, P2, ..., P n}, take 3 points at the horizontal adjacent positions of each point to form a point cloud set S, and calculate the smoothness:

[0016]

[0017] Where c is the smoothness, r1 is the Euclidean distance from point p1 in the point cloud to the lidar, and r2 is the Euclidean distance from point p2 in the point cloud to the lidar;

[0018] Set the smoothness threshold c1, divide the points with smoothness c greater than c1 into edge feature points and combine them to form a point cloud set F b , divide the points with smoothness c less than c1 into ground feature points and combine them to form a point cloud set F d , for the point cloud F d Filter out and get the point cloud set P1.

[0019] Further,

[0020] Obtain 3D point cloud data of different frames, perform point cloud segmentation and feature extraction in sequence, and obtain point cloud set P2;

[0021] Point cloud set P1 is used as the 3D point cloud set to be fused, and point cloud set P2 is used as the target 3D point cloud set;

[0022] The ICP method is used to solve the rotation matrix and translation vector from the 3D point cloud set to be fused to the target 3D point cloud set. The coordinates of the 3D point cloud set to be fused are changed by the rotation matrix and translation vector to obtain the transformed point cloud set P b , output positioning information.

[0023] Further,

[0024] Set the smoothness threshold c2;

[0025] Point Cloud Collection P b Take 2 points at the horizontal and vertical adjacent positions of each point to form a point cloud set S1, and calculate the smoothness c:

[0026] The points with smoothness c less than c2 are divided into non-bulkhead feature points and combined to form a point cloud set F c ;

[0027] Point Cloud Collection F c Filter out and get the final point cloud set P z .

[0028] Further,

[0029] Establish the corresponding relationship between the length of each hydraulic cylinder on the excavator and the angle value of the mechanical arm;

[0030] The length of each hydraulic cylinder on the excavator is measured by a displacement sensor, and the angle value of the mechanical arm is calculated based on the corresponding relationship between the length of each hydraulic cylinder on the excavator and the angle value of the mechanical arm;

[0031] The coordinate system is established with the center position of the rotary structure as the origin, and the state vector M of the bucket in the coordinate system is calculated based on the angle value of the mechanical arm. c ):

[0032]

[0033] In the formula, x is the coordinate value of the bucket in the x-axis direction, y is the coordinate value of the bucket in the y-axis direction, z is the coordinate value of the bucket in the z-axis direction, θ c is the rotation angle of the bucket, d0 is the distance from the first boom head end to the origin, a0 is the distance from the second boom head end to the first boom head end, a1 is the distance from the third boom head end to the second boom head end, a2 is the distance from the bucket head end to the third boom head end, a3 is the length of the bucket, θ0 is the rotation angle of the slewing structure, θ1 is the joint angle of the first boom, θ2 is the joint angle of the second boom, and θ3 is the joint angle of the third boom;

[0034] Calculate the horizontal distance between the bucket and the rotating structure based on the state vector of the bucket:

[0035]

[0036] Where L is the horizontal distance between the bucket and the rotary structure.

[0037] Further,

[0038] According to the depth map of the bulkhead, the distance from the laser radar to the bulkhead is calculated by the point-to-plane distance calculation method, and the distance between the bucket and the bulkhead is calculated in combination with the horizontal distance between the bucket and the rotating structure:

[0039]

[0040] Where s is the distance between the bucket and the bulkhead, l is the distance between the laser radar and the bulkhead, and r is the horizontal distance between the laser radar and the center of rotation.

[0041] Further,

[0042] Based on the different rotation angles of the rotary structure and the different joint angles of the mechanical arm, the bucket collision test was carried out to obtain the maximum and minimum spacing between the bucket and the bulkhead at the same excavator position and calculate the spacing difference:

[0043] Δs=s max -s min ;

[0044] Where Δs is the spacing difference, s max is the maximum distance between the bucket and the bulkhead, s min is the minimum distance between the bucket and the bulkhead.

[0045] Further,

[0046] Setting at least four sets of edge angle ranges: (d°, a°), (a°, b°), (b°, c°), (c°, d°);

[0047] In the point cloud set P z In the embodiment, the points in the edge angle range area (d°, a°) are divided into the first edge feature points and combined to form a sub-point cloud set F1, the points in the edge angle range area (a°, b°) are divided into the second edge feature points and combined to form a sub-point cloud set F2, the points in the edge angle range area (b°, c°) are divided into the third edge feature points and combined to form a sub-point cloud set F3, and the points in the edge angle range area (c°, d°) are divided into the fourth edge feature points and combined to form a sub-point cloud set F4;

[0048] Taking the spacing difference Δs as the displacement vector, the coordinate transformation is performed using the following formula to obtain the transformed virtual point cloud set P X :

[0049]

[0050] In the formula, is the transformed sub-point cloud set, and β is the angle between the point and the laser radar connection line.

[0051] Further,

[0052] Based on the bucket collision experiment, the rotation angle limit value of the rotation structure under different (L+s), the joint angle limit value of the robot arm, and the (L+s) limit value are obtained;

[0053] Based on the virtual point cloud set, the virtual bulkhead depth map is updated and the distance s between the bucket and the virtual bulkhead is recalculated. The L+s value is calculated, and the rotation angle limit value of the rotation structure and the joint angle limit value of the robotic arm corresponding to (L+s) are obtained to construct the anti-collision constraint:

[0054] γ>γ1andγ<γ2;

[0055] δ>δ1andδ<δ2;

[0056] Where s0 is the distance between the bucket and the bulkhead, γ1 and γ2 are the rotation angle limit values ​​corresponding to (L+s), γ is the rotation angle of the excavator, δ1 and δ2 are the joint angle limit values ​​of the manipulator corresponding to (L+s), δ is the joint angle of the manipulator, and L+s is the distance from the rotating structure to the virtual bulkhead.

[0057] In order to solve the above technical problems, the present invention further provides the following technical solutions: an anti-collision positioning system for an excavator in a cabin based on a three-dimensional point cloud, comprising:

[0058] LiDAR, mounted on top of the excavator;

[0059] A laser mapping module is connected to the laser radar for communication. The laser mapping module receives three-dimensional point cloud data of different frames, performs point cloud segmentation and feature extraction on the three-dimensional point cloud data in sequence, performs point cloud registration in combination with the three-dimensional point cloud data of different frames, and sets filtering adjustment to filter out non-ground and non-wall point clouds;

[0060] The sensor is installed on the slewing structure and mechanical arm of the excavator and is used to obtain the slewing angle of the lower slewing structure and the length of the hydraulic cylinder;

[0061] A statistical module is connected to the sensor for calculating the maximum and minimum spacing between the bucket and the bulkhead during the bucket collision experiment, and for calculating the rotation angle limit values ​​of the rotation structure and the joint angle limit values ​​of the mechanical arm at different spacings, and for calculating the spacing difference and using the spacing difference as a displacement vector to construct a virtual point cloud set;

[0062] An updating module, which is in communication with the statistical module, and updates the virtual bulkhead depth map based on the virtual point cloud set;

[0063] The control module is communicatively connected with the update module and the statistical module. The update module calculates the minimum spacing between the bucket and the virtual bulkhead according to the virtual bulkhead depth map, and establishes anti-collision constraints according to the rotation angle limit value of the rotation structure and the joint angle limit value of the robotic arm under different spacings.

[0064] Compared with the prior art, the present invention has the following beneficial effects:

[0065] In the present invention, a laser radar is used to obtain three-dimensional point cloud data, and data processing is performed to obtain a depth map of the bulkhead. In combination with the excavator model, the distance between the bucket and the bulkhead can be calculated, and a bucket collision experiment is performed to obtain the maximum and minimum spacings between the bucket and the bulkhead, and the spacing difference is calculated. A virtual point cloud set is constructed based on the spacing difference, and anti-collision constraints of a rotating structure and a mechanical arm are created based on the virtual point cloud set, thereby realizing the acquisition of the positioning information of the excavator itself, and being able to obtain the activity status of the mechanical arm and the rotating structure, and limiting the movement of the mechanical arm and the rotating structure through the virtual point cloud set, accurately obtaining the overall status of the excavator, and effectively ensuring the anti-collision of the entire excavator. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] In order to more clearly illustrate the implementation methods of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for the implementation methods or the description of the prior art. Obviously, the drawings in the following description are only exemplary, and for ordinary technicians in this field, other implementation drawings can be derived from the provided drawings without creative work.

[0067] Figure 1 A flowchart of a method for anti-collision positioning of an excavator in a cabin based on three-dimensional point cloud provided by an embodiment of the present invention;

[0068] Figure 2 is a flowchart of point cloud data processing and s value calculation in an embodiment of the present invention;

[0069] Figure 3 is a schematic structural diagram of an excavator in an embodiment of the present invention;

[0070] Figure 4 It is a schematic diagram of various structural lines of the excavator in the embodiment of the present invention in the coordinate axis;

[0071] Figure 5 It is a schematic diagram of the distance parameters of the excavator structure when the laser radar in the embodiment of the present invention is located on the rotating structure away from one end of the bulkhead;

[0072] Figure 6 It is a schematic diagram of the distance parameters of the excavator structure when the laser radar in the embodiment of the present invention is located on the rotating structure close to one end of the bulkhead;

[0073] Figure 7 Schematic diagram of the angle ranges of the edges on the horizontal plane of the cabin in an embodiment of the present invention;

[0074] Figure 8 is a schematic diagram of coordinate changes of a sub-point cloud set and a virtual sub-point cloud set in an embodiment of the present invention;

[0075] Fig. 9 is a schematic diagram of excavator structure distance parameters based on a virtual point cloud set in an embodiment of the present invention;

[0076] Fig.10 The distance between the rotary structure and the virtual bulkhead of the excavator in the embodiment of the present invention satisfies L+s=(L+s) min Schematic diagram of the bucket opening being adjusted to the maximum.

[0077] The numbers in the figure represent the following:

[0078] 1-rotating structure; 2-mechanical arm; 3-bucket;

[0079] 21-first boom; 22-second boom; 23-third boom. DETAILED DESCRIPTION

[0080] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0081] like Figure 1 and Figure 2 As shown, the present invention provides an anti-collision positioning method for an excavator in a cabin based on a three-dimensional point cloud, comprising the following steps:

[0082] The laser radar is used to scan the cabin environment to obtain three-dimensional point cloud data and map it onto a two-dimensional image plane to form a depth map;

[0083] The 3D point cloud data is segmented and features extracted in sequence, and the 3D point cloud data of different frames are combined for point cloud registration to output positioning information;

[0084] Each pixel point and its adjacent pixel points in the depth map form a point unit and perform feature extraction, and filter out non-cabin wall points that do not meet the smoothness standard to obtain the depth map of the bulkhead in the cabin;

[0085] Obtain the spatial parameters of the rotary structure and mechanical arm on the excavator to build an excavator model, and calculate the distance between the bucket and the bulkhead based on the excavator model and the bulkhead depth map;

[0086] A bucket collision experiment is carried out to obtain the maximum and minimum spacing between the bucket and the bulkhead, as well as the rotation angle limit values ​​of the rotating structure and the joint angle limit values ​​of the robotic arm at different spacings, and the spacing difference is calculated. The spacing difference is used as the displacement vector to construct a virtual point cloud set. Based on the virtual point cloud set, anti-collision constraints are established according to the rotation angle limit values ​​of the rotating structure and the joint angle limit values ​​of the robotic arm at different spacings.

[0087] In the present invention, a laser radar is used to obtain three-dimensional point cloud data, and data processing is performed to obtain a depth map of the bulkhead. In combination with the excavator model, the distance between the bucket and the bulkhead can be calculated, and a bucket collision experiment is performed to obtain the maximum and minimum spacings between the bucket and the bulkhead, and the spacing difference is calculated. A virtual point cloud set is constructed based on the spacing difference, and anti-collision constraints of a rotating structure and a mechanical arm are created based on the virtual point cloud set, thereby realizing the acquisition of the positioning information of the excavator itself, and being able to obtain the activity status of the mechanical arm and the rotating structure, and limiting the movement of the mechanical arm and the rotating structure through the virtual point cloud set, accurately obtaining the overall status of the excavator, and effectively ensuring the anti-collision of the entire excavator.

[0088] In the present invention, a laser radar with a 360° horizontal angle measurement is used to scan the cabin environment to obtain three-dimensional point cloud data, and then the three-dimensional point cloud data is processed.

[0089] In the present invention, point cloud segmentation and feature extraction are performed on three-dimensional point cloud data in sequence, including the following steps:

[0090] Divide the depth map into several sub-images in the horizontal direction;

[0091] For the point cloud P at time t t = {P1, P2, ..., P n}, take 3 points at the horizontal adjacent positions of each point to form a point cloud set S, and calculate the smoothness:

[0092]

[0093] Where c is the smoothness, r1 is the Euclidean distance from point p1 in the point cloud to the lidar, and r2 is the Euclidean distance from point p2 in the point cloud to the lidar;

[0094] Set the smoothness threshold c1, divide the points with smoothness c greater than c1 into edge feature points and combine them to form a point cloud set F b , divide the points with smoothness c less than c1 into ground feature points and combine them to form a point cloud set F d , for the point cloud F d Filter out and get the point cloud set P1.

[0095] The above embodiment can filter out ground feature points in a point cloud set, and is a process of processing one frame of three-dimensional point cloud data.

[0096] In order to improve the accuracy of the final depth map, it is necessary to obtain 3D point cloud data of different frames, perform point cloud segmentation and feature extraction in sequence, and obtain point cloud set P2;

[0097] Point cloud set P1 is used as the 3D point cloud set to be fused, and point cloud set P2 is used as the target 3D point cloud set;

[0098] The ICP method is used to solve the rotation matrix and translation vector from the 3D point cloud set to be fused to the target 3D point cloud set. The coordinates of the 3D point cloud set to be fused are changed by the rotation matrix and translation vector to obtain the transformed point cloud set P b , output positioning information.

[0099] Through the above process, point cloud registration of three-dimensional point cloud data of different frames can be completed to achieve graphics optimization.

[0100] Among them, ICP is a commonly used registration method. The shapes of the three-dimensional point cloud data of different frames to be registered are slightly different. ICP is used to register two frames of three-dimensional point cloud data to obtain accurate rotation matrix and translation vector. The rotation matrix and translation vector can be used to perform coordinate transformation to complete the point cloud registration and obtain the transformed point cloud set P. b .

[0101] Get point cloud set P b Then perform secondary feature extraction on the point cloud set. The specific steps are as follows:

[0102] Set the smoothness threshold c2;

[0103] Point Cloud Collection P b Take 2 points at the horizontal and vertical adjacent positions of each point to form a point cloud set S1, and calculate the smoothness c:

[0104] The points with smoothness c less than c2 are divided into non-bulkhead feature points and combined to form a point cloud set F c ;

[0105] Point Cloud Collection F c Filter out and get the final point cloud set P z .

[0106] The second feature extraction process can filter out the point cloud set P b The point cloud of the central non-bulkhead is obtained to obtain the final point cloud set P z , which is the bulkhead depth diagram in the cabin.

[0107] like Figure 3The figure shows a schematic diagram of the structure of an excavator. The present invention obtains the spatial parameters of the rotary structure and the mechanical arm on the excavator for constructing an excavator model. The excavator model can be used to establish the corresponding relationship between the length of each hydraulic cylinder on the excavator and the angle value of the mechanical arm. The rotary structure and the mechanical arm in the excavator model are converted into lines with variable coordinates and angles. The corresponding relationship between the length of each hydraulic cylinder on the excavator and the angle value of the mechanical arm is established through the quantitative relationship between each line and angle. In addition, the corresponding relationship between the length of each hydraulic cylinder on the excavator and the angle value of the mechanical arm can also be deeply analyzed through a bucket collision experiment. Based on deep learning, the corresponding relationship between the length of each hydraulic cylinder on the excavator and the angle value of the mechanical arm is obtained through a sample set of angle values ​​of the mechanical arm corresponding to different hydraulic cylinder lengths.

[0108] The length of each hydraulic cylinder on the excavator is measured by a displacement sensor, and the length of the hydraulic cylinder is substituted into the corresponding relationship between the length of each hydraulic cylinder on the excavator and the angle value of the mechanical arm to obtain the angle value of the mechanical arm;

[0109] like Figure 4 As shown in the figure, a coordinate system is established with the center position of the rotary structure as the origin, and the state vector M of the bucket in the coordinate system is calculated based on the angle value of the mechanical arm. c ):

[0110]

[0111] In the formula, x is the coordinate value of the bucket in the x-axis direction, y is the coordinate value of the bucket in the y-axis direction, z is the coordinate value of the bucket in the z-axis direction, θ c is the rotation angle of the bucket, d0 is the distance from the first boom head end to the origin, a0 is the distance from the second boom head end to the first boom head end, a1 is the distance from the third boom head end to the second boom head end, a2 is the distance from the bucket head end to the third boom head end, a3 is the length of the bucket, θ0 is the rotation angle of the slewing structure, θ1 is the joint angle of the first boom, θ2 is the joint angle of the second boom, and θ3 is the joint angle of the third boom;

[0112] Calculate the horizontal distance between the bucket and the rotating structure based on the state vector of the bucket:

[0113]

[0114] Where L is the horizontal distance between the bucket and the rotary structure.

[0115] In the above embodiments, the horizontal distance between the bucket and the rotary structure is finally calculated by sequentially acquiring the length of the hydraulic cylinder, calculating the angle value of the mechanical arm, and calculating the state vector of the bucket.

[0116] Among them, the mechanical arm is composed of a first movable arm, a second movable arm, and a second movable arm in sequence.

[0117] After calculating the horizontal distance between the bucket and the rotating structure, the distance from the laser radar to the bulkhead is calculated using the point-to-plane distance calculation method based on the bulkhead depth map, and the distance between the bucket and the bulkhead is inferred based on the horizontal distance between the bucket and the rotating structure:

[0118]

[0119] Where s is the distance between the bucket and the bulkhead, l is the distance between the laser radar and the bulkhead, and r is the horizontal distance between the laser radar and the center of rotation.

[0120] In practical applications, the laser radar is installed in different positions in different situations, such as Figure 6 As shown in , if the laser radar is closer to the bulkhead relative to the center of the rotating structure, then s = l + rL, as Figure 5 As shown, if the laser radar is farther away from the bulkhead relative to the center of the rotating structure, then s=lrL.

[0121] In the present invention, many bucket collision tests need to be conducted before the excavator works. Bucket collision tests are conducted based on different rotation angles of the rotary structure and different joint angles of the mechanical arm to obtain the maximum and minimum spacing between the bucket and the bulkhead at the same excavator position and calculate the spacing difference:

[0122] Δs=s max -s min ;

[0123] Where Δs is the spacing difference, s max is the maximum distance between the bucket and the bulkhead, s min is the minimum distance between the bucket and the bulkhead.

[0124] The bucket collision test is based on different (L+s), different rotation angles of the rotating structure, and different joint angles of the robotic arm. That is to say, in repeated bucket collision tests, different combinations of (L+s), rotation angle limit values ​​of the rotating structure, and joint angle limit values ​​of the robotic arm can be obtained.

[0125] Among them, for a certain (L+s), it does not correspond to a rotation angle limit value of a rotating structure or a joint angle limit value of a robotic arm, but corresponds to different joint angle limit values ​​at different rotation angles, and corresponds to different rotation angle limit values ​​at different joint angles.

[0126] In practical applications, the corresponding joint angle limit values ​​and rotation angle limit values ​​can be used for anti-collision constraints.

[0127] In the present invention, the specific method of constructing a virtual point cloud set includes the following steps:

[0128] like Figure 7 As shown, at least four groups of edge angle ranges are set: (d°, a°), (a°, b°), (b°, c°), (c°, d°);

[0129] In the point cloud set P z In the embodiment, the points in the edge angle range area (d°, a°) are divided into the first edge feature points and combined to form a sub-point cloud set F1, the points in the edge angle range area (a°, b°) are divided into the second edge feature points and combined to form a sub-point cloud set F2, the points in the edge angle range area (b°, c°) are divided into the third edge feature points and combined to form a sub-point cloud set F3, and the points in the edge angle range area (c°, d°) are divided into the fourth edge feature points and combined to form a sub-point cloud set F4;

[0130] Taking the spacing difference Δs as the displacement vector, the coordinate transformation is performed using the following formula to obtain the transformed virtual point cloud set P X :

[0131]

[0132] In the formula, is the transformed sub-point cloud set, and β is the angle between the point and the laser radar connection line.

[0133] In the above embodiments, point cloud cutting is performed for different edge angle ranges to form different sub-point cloud sets. Taking the cabin as a rectangular parallelepiped as an example, there are four cabin walls, and each cabin wall has a different edge angle range (d°, a°), (a°, b°), (b°, c°), and (c°, d°). The point cloud set is divided into four sub-point cloud sets corresponding to different edge angle ranges.

[0134] For excavators in different positions, the corresponding edge angle ranges are different, and the specific changes can be calculated and adjusted according to the specific position of the excavator.

[0135] After the point cloud set is divided into sub-point cloud sets, coordinate changes are performed in different sub-point cloud sets to construct virtual sub-point cloud sets, and the virtual sub-point cloud sets are combined to form a virtual point cloud set virtual point cloud set P X .

[0136] like Figure 8As shown, from a top-down perspective, the solid line part can be regarded as the initial sub-point cloud set, and the dotted line part can be regarded as a virtual sub-point cloud set. The sub-point cloud set in the edge angle range (d°, a°) moves Δs in the negative direction of the x-axis to form a virtual sub-point cloud set, the sub-point cloud set in the edge angle range (a°, b°) moves Δs in the negative direction of the y-axis to form a virtual sub-point cloud set, the sub-point cloud set in the edge angle range (b°, c°) moves Δs in the positive direction of the x-axis to form a virtual sub-point cloud set, and the sub-point cloud set in the edge angle range (c°, d°) moves Δs in the positive direction of the y-axis to form a virtual sub-point cloud set. The four virtual sub-point cloud sets are combined to form a virtual point cloud set.

[0137] After obtaining the virtual point cloud set, the rotation angle limit value of the rotation structure, the joint angle limit value of the robot arm, and the (L+s) limit value under different (L+s) are obtained based on the bucket collision experiment;

[0138] Based on the virtual point cloud set, the virtual bulkhead depth map is updated and the distance s between the bucket and the virtual bulkhead is recalculated. The L+s value is calculated, and the rotation angle limit value of the rotation structure and the joint angle limit value of the robotic arm corresponding to (L+s0) are obtained to construct the anti-collision constraint:

[0139] γ>γ1andγ<γ2;

[0140] δ>δ1andδ<δ2;

[0141] Where s0 is the distance between the bucket and the bulkhead, γ1 and γ2 are the rotation angle limit values ​​corresponding to (L+s), γ is the rotation angle of the excavator, δ1 and δ2 are the joint angle limit values ​​of the mechanical arm corresponding to (L+s), δ is the joint angle of the mechanical arm, L+s is the distance from the rotation structure to the bulkhead, (L+s) min is the limiting minimum value of (L+s).

[0142] In the above embodiment, after constructing the virtual point cloud set, the virtual point cloud set is used as a reference, such as Fig. 9 As shown in the figure, the vertical double dashed line is the position corresponding to the virtual point cloud set, l is adjusted to the distance from the laser radar to the virtual bulkhead, and (L+s) is adjusted accordingly. The (L+s) value is relative to the virtual point cloud set. Assuming that the joint angle limit values ​​relative to the physical bulkhead under the (L+s) value are δ1, δ2, and the rotation angle limit values ​​are γ1, γ2, the anti-collision constraint of the virtual bulkhead formed by the virtual point cloud set is constructed with the joint angle limit values ​​and the rotation angle limit values.

[0143] The virtual point cloud corresponds to the virtual bulkhead. That is to say, after the bucket collision test, the joint angle limit value and the rotation angle limit value under the (L+s) value can be obtained based on the physical bulkhead, and they are directly applied to the virtual bulkhead. When the distance between the rotating structure and the virtual bulkhead is at the (L+s) value, the corresponding joint angle limit value and the rotation angle limit value are also possessed. The corresponding anti-collision constraints are constructed, which can effectively avoid the collision between the entire excavator and the physical bulkhead.

[0144] Virtual point cloud can achieve collision avoidance by increasing the distance between the bucket and the physical bulkhead. The above collision avoidance constraints may make it impossible to transfer materials between the virtual bulkhead and the physical bulkhead. The specific design can be determined according to the actual situation. If a large amount of materials are found to be accumulated at the edge of the cabin during the transfer process and the materials cannot be transferred under the above collision constraints, the following design can also be made:

[0145] In the bucket collision experiment, obtain the minimum value (L+s) under the solid bulkhead min , create the following new collision avoidance constraints:

[0146] L+s>(L+s) min ;

[0147] In the above embodiment, the distance between the rotating structure and the virtual bulkhead is greater than (L+s) min ;

[0148] Assume L+s=(L+s) min , ignoring the size of the bucket itself, when the bucket opening is the smallest, the distance between the bucket and the virtual wall is 0, at which time the distance between the bucket and the rotary structure is the smallest, and the distance between the bucket and the physical bulkhead is the largest;

[0149] Since Δs=s max -s min In, min Generally it is 0 (ignoring the size of the bucket itself), then Δs=s max ;

[0150] like Fig.10 As shown, when the bucket is adjusted to: the distance between the bucket and the solid bulkhead is the smallest, the distance between the bucket and the rotating structure is the largest, the bucket opening is the largest (the horizontal distance is the farthest), and it just moves a distance of Δs in the horizontal direction, and at this time it just touches the solid bulkhead.

[0151] Therefore, the anti-collision constraint L+s>(L+s) is established min It can effectively ensure that the bucket does not collide with the bulkhead when it is at its maximum opening, thus achieving anti-collision while also expanding the material transfer range.

[0152] The present invention also provides a positioning system for an anti-collision positioning method of an excavator in a cabin based on a three-dimensional point cloud, which has:

[0153] LiDAR, mounted on top of the excavator;

[0154] A laser mapping module is connected to the laser radar for communication. The laser mapping module receives three-dimensional point cloud data of different frames, performs point cloud segmentation and feature extraction on the three-dimensional point cloud data in sequence, performs point cloud registration in combination with the three-dimensional point cloud data of different frames, and sets filtering adjustment to filter out non-ground and non-wall point clouds;

[0155] The sensor is installed on the slewing structure and mechanical arm of the excavator and is used to obtain the slewing angle of the lower slewing structure and the length of the hydraulic cylinder;

[0156] A statistical module is connected to the sensor for calculating the maximum and minimum spacing between the bucket and the bulkhead during the bucket collision experiment, and for calculating the rotation angle limit values ​​of the rotation structure and the joint angle limit values ​​of the mechanical arm at different spacings, and for calculating the spacing difference and using the spacing difference as a displacement vector to construct a virtual point cloud set;

[0157] An updating module, which is in communication with the statistical module, and updates the virtual bulkhead depth map based on the virtual point cloud set;

[0158] The control module is communicatively connected with the update module and the statistical module. The update module calculates the minimum spacing between the bucket and the virtual bulkhead according to the virtual bulkhead depth map, and establishes anti-collision constraints according to the rotation angle limit value of the rotation structure and the joint angle limit value of the robotic arm under different spacings.

[0159] Among them, the sensors include displacement sensors and posture sensors. The displacement sensor can detect the length of the hydraulic cylinder, and the posture sensor can detect the joint angle.

[0160] The above embodiments are only exemplary embodiments of the present application and are not intended to limit the present application. The protection scope of the present application is defined by the claims. Those skilled in the art may make various modifications or equivalent substitutions to the present application within the essence and protection scope of the present application, and such modifications or equivalent substitutions shall also be deemed to fall within the protection scope of the present application.

Claims

1. A collision avoidance positioning method for an excavator in a cabin based on three-dimensional point cloud, characterized in that: The following steps are involved: The laser radar is used to scan the cabin environment to obtain three-dimensional point cloud data and map it onto a two-dimensional image plane to form a depth map; The 3D point cloud data is segmented and features extracted in sequence, and the 3D point cloud data of different frames are combined for point cloud registration to output positioning information; Each pixel point and its adjacent pixel points in the depth map form a point unit and perform feature extraction, and filter out non-cabin wall points that do not meet the smoothness standard to obtain the depth map of the bulkhead in the cabin; Obtain the spatial parameters of the rotary structure and mechanical arm on the excavator to build an excavator model, and calculate the distance between the bucket and the bulkhead based on the excavator model and the bulkhead depth map; A bucket collision experiment is carried out to obtain the maximum and minimum spacing between the bucket and the bulkhead, as well as the rotation angle limit values ​​of the rotating structure and the joint angle limit values ​​of the robotic arm at different spacings, and the spacing difference is calculated. The spacing difference is used as the displacement vector to construct a virtual point cloud set. Based on the virtual point cloud set, anti-collision constraints are established according to the rotation angle limit values ​​of the rotating structure and the joint angle limit values ​​of the robotic arm at different spacings.

2. The anti-collision positioning method for an excavator in a cabin based on three-dimensional point cloud according to claim 1 is characterized in that: Point cloud segmentation and feature extraction are performed on the three-dimensional point cloud data in sequence, including the following steps: Divide the depth map into several sub-images in the horizontal direction; For the point cloud P at time t t = {P1, P2, ..., P n }, take 3 points at the horizontal adjacent positions of each point to form a point cloud set S, and calculate the smoothness: Where c is the smoothness, r1 is the Euclidean distance from point p1 in the point cloud to the lidar, and r2 is the Euclidean distance from point p2 in the point cloud to the lidar; Set the smoothness threshold c1, divide the points with smoothness c greater than c1 into edge feature points and combine them to form a point cloud set F b , divide the points with smoothness c less than c1 into ground feature points and combine them to form a point cloud set F d , for the point cloud F d Filter out and get the point cloud set P1.

3. The anti-collision positioning method for excavators in a ship cabin based on three-dimensional point cloud according to claim 2 is characterized in that: Obtain 3D point cloud data of different frames, perform point cloud segmentation and feature extraction in sequence, and obtain point cloud set P2; Point cloud set P1 is used as the 3D point cloud set to be fused, and point cloud set P2 is used as the target 3D point cloud set; The ICP method is used to solve the rotation matrix and translation vector from the 3D point cloud set to be fused to the target 3D point cloud set. The coordinates of the 3D point cloud set to be fused are changed by the rotation matrix and translation vector to obtain the transformed point cloud set P b , output positioning information.

4. The anti-collision positioning method for an excavator in a cabin based on three-dimensional point cloud according to claim 3 is characterized in that: Set the smoothness threshold c2; Point Cloud Collection P b Take 2 points at the horizontal and vertical adjacent positions of each point to form a point cloud set S1, and calculate the smoothness c: The points with smoothness c less than c2 are divided into non-bulkhead feature points and combined to form a point cloud set F c ; Point Cloud Collection F c Filter out and get the final point cloud set P z .

5. The anti-collision positioning method for excavators in a ship cabin based on three-dimensional point cloud according to claim 4 is characterized in that: Establish the corresponding relationship between the length of each hydraulic cylinder on the excavator and the angle value of the mechanical arm; The length of each hydraulic cylinder on the excavator is measured by a displacement sensor, and the angle value of the mechanical arm is calculated based on the corresponding relationship between the length of each hydraulic cylinder on the excavator and the angle value of the mechanical arm; The coordinate system is established with the center position of the rotary structure as the origin, and the state vector M of the bucket in the coordinate system is calculated based on the angle value of the mechanical arm. c ): In the formula, x is the coordinate value of the bucket in the x-axis direction, y is the coordinate value of the bucket in the y-axis direction, z is the coordinate value of the bucket in the z-axis direction, θ c is the rotation angle of the bucket, d0 is the distance from the first boom head end to the origin, a0 is the distance from the second boom head end to the first boom head end, a1 is the distance from the third boom head end to the second boom head end, a2 is the distance from the bucket head end to the third boom head end, a3 is the length of the bucket, θ0 is the rotation angle of the slewing structure, θ1 is the joint angle of the first boom, θ2 is the joint angle of the second boom, and θ3 is the joint angle of the third boom; Calculate the horizontal distance between the bucket and the rotating structure based on the state vector of the bucket: Where L is the horizontal distance between the bucket and the rotary structure.

6. The anti-collision positioning method for an excavator in a ship cabin based on three-dimensional point cloud according to claim 5, characterized in that: According to the depth map of the bulkhead, the distance from the laser radar to the bulkhead is calculated by the point-to-plane distance calculation method, and the distance between the bucket and the bulkhead is calculated in combination with the horizontal distance between the bucket and the rotating structure: Where s is the distance between the bucket and the bulkhead, l is the distance between the laser radar and the bulkhead, and r is the horizontal distance between the laser radar and the center of rotation.

7. The anti-collision positioning method for an excavator in a ship cabin based on three-dimensional point cloud according to claim 6, characterized in that: Based on the different rotation angles of the rotary structure and the different joint angles of the mechanical arm, the bucket collision test was carried out to obtain the maximum and minimum spacing between the bucket and the bulkhead at the same excavator position and calculate the spacing difference: Δs=s m -s min ; In the formula, Δs is the spacing difference, s max is the maximum distance between the bucket and the bulkhead, s min is the minimum distance between the bucket and the bulkhead.

8. The anti-collision positioning method for an excavator in a ship cabin based on three-dimensional point cloud according to claim 7, characterized in that: Setting at least four sets of edge angle ranges: (d°, a°), (a°, b°), (b°, c°), (c°, d°); In the point cloud set P z In the embodiment, the points in the edge angle range area (d°, a°) are divided into the first edge feature points and combined to form a sub-point cloud set F1, the points in the edge angle range area (a°, b°) are divided into the second edge feature points and combined to form a sub-point cloud set F2, the points in the edge angle range area (b°, c°) are divided into the third edge feature points and combined to form a sub-point cloud set F3, and the points in the edge angle range area (c°, d°) are divided into the fourth edge feature points and combined to form a sub-point cloud set F4; Taking the spacing difference Δs as the displacement vector, the coordinate transformation is performed using the following formula to obtain the transformed virtual point cloud set P X : In the formula, is the transformed sub-point cloud set, and β is the angle between the point and the laser radar connection line.

9. The anti-collision positioning method for an excavator in a ship cabin based on three-dimensional point cloud according to claim 8, characterized in that: Based on the bucket collision experiment, the rotation angle limit value of the rotation structure, the joint angle limit value of the robot arm and the (L+s) limit value under different (L+s) are obtained; Based on the virtual point cloud set, the virtual bulkhead depth map is updated and the distance s between the bucket and the virtual bulkhead is recalculated. The L+s value is calculated, and the rotation angle limit value of the rotation structure and the joint angle limit value of the robotic arm corresponding to (L+s) are obtained to construct the anti-collision constraint: γ>γ1andγ<γ2; δ>δ1andδ<δ2; Where s0 is the distance between the bucket and the bulkhead, γ1 and γ2 are the rotation angle limit values ​​corresponding to (L+s), γ is the rotation angle of the excavator, δ1 and δ2 are the joint angle limit values ​​of the manipulator corresponding to (L+s), δ is the joint angle of the manipulator, and L+s is the distance from the rotating structure to the virtual bulkhead.

10. A positioning system using the anti-collision positioning method for an excavator in a ship cabin based on three-dimensional point cloud as claimed in any one of claims 1 to 9, characterized in that: have: LiDAR, mounted on top of the excavator; A laser mapping module is connected to the laser radar for communication. The laser mapping module receives three-dimensional point cloud data of different frames, performs point cloud segmentation and feature extraction on the three-dimensional point cloud data in sequence, performs point cloud registration in combination with the three-dimensional point cloud data of different frames, and sets filtering adjustment to filter out non-ground and non-wall point clouds; The sensor is installed on the slewing structure and mechanical arm of the excavator and is used to obtain the slewing angle of the lower slewing structure and the length of the hydraulic cylinder; A statistical module is connected to the sensor for calculating the maximum and minimum spacing between the bucket and the bulkhead during the bucket collision experiment, and for calculating the rotation angle limit values ​​of the rotation structure and the joint angle limit values ​​of the mechanical arm at different spacings, and for calculating the spacing difference and using the spacing difference as a displacement vector to construct a virtual point cloud set; An updating module, which is in communication with the statistical module, and updates the virtual bulkhead depth map based on the virtual point cloud set; The control module is communicatively connected with the update module and the statistical module. The update module calculates the minimum spacing between the bucket and the virtual bulkhead according to the virtual bulkhead depth map, and establishes anti-collision constraints according to the rotation angle limit value of the rotation structure and the joint angle limit value of the robotic arm under different spacings.

Images