A method for detecting falls and low obstacles based on solid-state line laser

Abstract

The application provides a kind of based on solid-state line laser's drop and low obstacle detection method, belong to robot application technical field.The detection equipment of the present application is equipped with more than 2 line laser sensors, the line laser sensor is vertically installed, the based on solid-state line laser's drop and low obstacle detection method includes the following steps: obtaining the detection data of the line laser sensor;The point detected under each line laser sensor coordinate system is converted into the position point under the detection equipment coordinate system with the detection equipment as the origin;Drop detection and low obstacle detection are carried out, the beam of line laser sensor is segmented, if there is initial ground, the straightness error of each segmented beam is calculated, then the error between segmented beam and initial ground is calculated, when the sum of straightness error and error is less than second set value, it is marked as ground, otherwise it is marked as obstacle.The present application can eliminate the data ambiguity of sensor in slope scene, improve the safety of equipment autonomous operation.

Description

Technical Field

[0001] This invention relates to the field of robotics application technology, specifically to a method for detecting falls and low obstacles based on solid-state line lasers. Background Technology

[0002] Indoor mobile robots typically rely on sensors such as single-line LiDAR and RGBD cameras to detect surrounding obstacles and avoid collisions during movement. Due to the limited field of view (FOV) of these sensors, they generally cannot cover a 360° area around the robot. Some robotic vacuum cleaners use solid-state line lasers for blind spot detection, but these are usually installed horizontally. While this maximizes sensor coverage, in complex scenarios, such as when there is a slope in front of the robot, the solid-state line lasers, all positioned at a single height, cannot perform cross-sensing from multiple angles, often leading to false detections. Summary of the Invention

[0003] To address the problems in the prior art, this invention provides a method for detecting drops and low obstacles based on solid-state line lasers.

[0004] This invention relates to a method for detecting drops and low obstacles based on solid-state line lasers. The detection device is equipped with two or more line laser sensors, which are vertically mounted. The method for detecting drops and low obstacles based on solid-state line lasers includes the following steps:

[0005] S1: Acquire the detection data from the line laser sensor;

[0006] S2: Convert the points detected in the coordinate system of each line laser sensor into position points in the coordinate system of the detection device with the detection device as the origin;

[0007] S3: Perform drop detection and low obstacle detection, among which...

[0008] Drop detection: When a valid location point in the drop detection zone is detected, the difference between the distance the line laser should have hit the ground and the actual distance it hit the ground is calculated. When the difference exceeds a first set value, it is marked as a drop.

[0009] Obstacle detection: For the beam segmentation of the line laser sensor, if there is an initial ground, calculate the straightness error of each beam segment, and then calculate the error between the beam segment and the initial ground. When the sum of the straightness error and the error is less than a second set value, it is marked as ground; otherwise, it is marked as an obstacle.

[0010] Furthermore, the detection device is a walking robot, and there are two line laser sensors, which are respectively installed at predetermined positions on the left and right sides in front of the robot in the direction of movement.

[0011] Further, in step S1, the points collected by the line laser sensor are in the form of a data set, with the data format being: [p1, p2, ..., p...]. f ], where f represents the number of points in each frame of data, and each point is represented by the data format p. i =[θ i , l i ], where p i For the i-th data point, which is the distance l of the nearest object measured by the i-th line beam of the line laser sensor. i and the corresponding angle θ i When there is no object within the detection range of the line laser sensor, the distance l of the nearest object within the range of the line laser sensor is... i =0.

[0012] Further, in step S2, one of the points on the detection device is set as the origin of the detection device coordinate system, and a three-dimensional coordinate system is constructed, where the x-axis points in the forward direction, the y-axis is horizontal, and the z-axis is vertical. The method for converting the points detected in each line laser sensor coordinate system into position points in the detection device coordinate system with the detection device as the origin is as follows:

[0013] Data p from the line laser sensor i =[θ i , l i The corresponding point in the online laser coordinate system is: l p i =[l i cos(θ i ),l i sin(θ i ),0],

[0014] The position points converted to the coordinate system of the detection equipment are: in, l p i Let be the coordinates of the point detected by the i-th line laser sensor in the line laser sensor coordinate system. b p i Let be the coordinates of the point detected by the laser sensor along the i-th line in the coordinate system of the detection device. Let be the rotation matrix from the line laser sensor coordinate system to the detection device coordinate system. It is a translation vector.

[0015] Furthermore, in step S3, during drop detection, when the difference exceeds a first preset value, it is marked as before a drop. The process also includes a sub-step for filtering the drop region. The method for this sub-step is as follows:

[0016] A1: Find the first bundle m marked as fallen;

[0017] A2: Find the next bundle n marked as fallen;

[0018] A3: Determine if the difference between n and m is greater than the third set value. If not, mark all wire harnesses m and n as not falling. If yes, proceed to the next step.

[0019] A4: Increase the total number of consecutive falls by 1;

[0020] A5: Determine if the total number of consecutive drops exceeds the threshold. If so, mark all wire harnesses mn as dropped.

[0021] Furthermore, the specific processing method for the drop detection is as follows:

[0022] (1) For harness i, calculate the ground that it should detect. ground p i ;

[0023] (2) The direction vector of beam i in the online laser coordinate system is: l n i =[cos(θ) i ),sin(θ i ),0] transformed into the direction vector in the coordinate system of the detection equipment b n i for: The point hit by the corresponding beam is: Where r is the length of the vector;

[0024] (3) By ground p i [2] = 0, therefore: in, ground p i [2] is the height of the ground hit by the wire harness. This is the last element of the 3*1 translation vector. b n i [2] is the last element of the 3*1 direction vector;

[0025] (4) Based on vector r, from the formula:

[0026]

[0027] Get the ground point that should be hit ground p i ;

[0028] (5) When l i =0 or |l i If -r is greater than the first set value, it is marked as a fall; otherwise, it is marked as no fall.

[0029] Furthermore, in step S3, during the obstacle detection step, if there is no initial ground, then all points detected by the segmented wire harness are marked as non-ground.

[0030] The method for segmenting the wire harness is as follows:

[0031] (1) The original setting is that the range of the detection ground wire bundle is i∈[s ground e ground ], where s ground =0,e ground Determined based on the obstacle-crossing capability of the detection equipment;

[0032] (2) Based on the detected drop areas, these drop areas are excluded from the ground detection area, resulting in the actual ground detection area.

[0033] In each wire harness segment k, the actual ground detection area k∈[1, 2, ..., m], is split according to the set maximum length to obtain a new actual ground detection area. n represents the total number of new actual ground detection areas.

[0034] Furthermore, the initial ground detection method is as follows:

[0035] Traverse each region The ground score for each region is calculated sequentially. When a set threshold is met, it indicates that the initial ground region has been found.

[0036] For each region, the method for calculating the ground score is as follows:

[0037] (1) Convert the points within the area to their positions in the coordinate system of the detection equipment;

[0038] (2) Use p to detect points at infinity. inf Fill in the blanks to obtain the sequence P = [ b p1, ... b p k ], where k = ji, if the sequence P contains non-p inf If the number of points is less than the threshold, the area is determined to be a non-ground area.

[0039] (3) Extract the height of the position points in sequence P, and take the absolute value to obtain the difference P between the height and the ideal ground. z P z =[ b p1[2]],| b p2[2]|,...,| b p k [2]|],where,| b p k[2]| represents the height of the k-th position;

[0040] (4) Statistical P z The mean E[P z ], and variance D[P z ],if If the value is less than the set value, it means that the initial ground has been found.

[0041] Furthermore, if the initial ground is detected, the method for obtaining the straight line of the initial ground is as follows:

[0042] (1) Regarding the initial ground The corresponding sequence P in the coordinate system of the detection equipment is: P = [ b p1,... b p k Extract the straight line from it;

[0043] (2) Transform sequence P to a 2D loz coordinate system with the origin of the coordinate system of the device under test as the origin, where, z = b p k [2], the obtained sequence P loz for:

[0044] P loz =[ lo2 p1, loz p2, ..., loz p k ]

[0045] in, loz p k This is the reading of the k-th point in the sequence in the loz 2D coordinate system.

[0046] (3) Linear fitting:

[0047] Assuming the equation of the line is y = ux + v, then construct matrix A. Straightness is the coefficient of straightness. loz p1[0] is the first element of the reading of the first point in the sequence under the loz two-dimensional coordinate system. loz p1[1] is the second element of the reading of the first point in the sequence under the loz two-dimensional coordinate system.

[0048] Furthermore, after obtaining the initial straight line of the ground, the specific processing method for obstacle detection is as follows:

[0049] (1) Regarding the initial ground Then, for the ground detection area where k > m, the ground detection area of ​​the k-th wire bundle is... The corresponding sequence is

[0050] (2) Construct A, x, y for the ground detection area, and construct the straightness deviation e1 of the segmented beam, e1 = A(A T A) -1 A T yy, and the difference e2 between the constructed and initialized ground straightness coefficients. in, The straightness coefficient of the initial ground surface. The coefficients of the fitted straight line for the ground detection area of ​​the wire harness k;

[0051] (3) When ||e1||2+||e2||2 is less than the second set value, it means that the current detection area is all ground; otherwise, the current detection area is not all ground.

[0052] (4) When the current detection area is not entirely the ground, then traverse the candidate detection areas. An inner point, whose coordinates are log p k Find the point with the largest straight-line distance *d* from the candidate detection region. The distance *d* is calculated as follows: p[0] is the first element of each point, and p[1] is the second element of each point within the detection area;

[0053] (5) This detection area Divided into two regions Then obstacle detection is performed.

[0054] Compared with the prior art, the beneficial effects of the present invention are: two or more line laser sensors are set vertically in front of the device under test, and blind spot compensation can be achieved by simply using line laser sensors. If more of them are set on the detection device, the customer can achieve a 360-degree detection range at a lower cost.

[0055] By segmenting the beam of the line laser sensor for detection, the ambiguity of the sensor data in slope scenarios can be eliminated. In addition, the detection method of this invention can effectively detect falls and low obstacles, improving the safety of autonomous operation of mobile detection equipment. Attached Figure Description

[0056] To more clearly illustrate the solutions in this invention or the prior art, the accompanying drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0057] Figure 1This is a schematic diagram of the installation of an embodiment of the line laser sensor in the detection equipment of the present invention;

[0058] Figure 2 Schematic diagram of the detection range of the detection equipment of this invention;

[0059] Figure 3 This is a flowchart of the detection method of the present invention;

[0060] Figure 4 This is a schematic diagram of the coordinate system structure of the detection equipment;

[0061] Figure 5 A flowchart illustrating an embodiment of drop detection method;

[0062] Figure 6 This is a flowchart of an example of obstacle detection method. Detailed Implementation

[0063] Unless otherwise defined, all technical and scientific terms used in this invention have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains; the terminology used in this specification is for the purpose of describing particular embodiments only and is not intended to limit the invention; the terms "comprising" and "having," and any variations thereof, in the specification, claims, and foregoing drawings are intended to cover non-exclusive inclusion. The terms "first," "second," etc., in the specification, claims, or foregoing drawings are used to distinguish different objects, not to describe a particular order.

[0064] In this invention, the reference to "embodiment" means that a specific feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of the invention. The appearance of this phrase in various places in the specification does not necessarily refer to the same embodiment, nor is it a mutually exclusive, independent, or alternative embodiment to other embodiments. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described in this invention can be combined with other embodiments.

[0065] To enable those skilled in the art to better understand the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings.

[0066] Mobile detection devices, such as indoor mobile robots, often face challenges beyond flat surfaces. Small slopes and small objects that have fallen to the ground are also common. To achieve automated operation in complex indoor environments, multiple sensors are needed to cover the robot's 360° detection range. While solid-state line lasers have limited detection distance, they are relatively inexpensive. This example uses an indoor mobile robot and a minimal number of line laser sensors for illustration. Of course, increasing the number of line laser sensors and using multiple sensors in combination can cover the robot's 360° detection range.

[0067] like Figure 1 and Figure 2 As shown, in this invention, one line laser sensor 2 and 3 are vertically installed at designated positions on the left and right sides in front of the robot 1 in the direction of travel. The scanning angle of one line laser sensor 3 is 100 degrees and the maximum detection distance is 0.3m. Therefore, in order to overcome the limited detection range of the line laser sensor, this example uses two line laser sensors 2 and 3 to expand the coverage area in front. Furthermore, the detection results of the two line laser sensors 2 and 3 can corroborate each other, improve the accuracy of detection, and also fill blind spots.

[0068] like Figure 3 As shown, the drop and low obstacle detection method based on solid-state line laser of this invention is as follows:

[0069] Step S1: Obtain the detection data from the line laser sensor.

[0070] In this step, the points collected by the line laser sensor are in the form of a data set, with the data format being: [p1, p2, ..., p...]. f ], where f represents the number of points in each frame of data, and each point is represented by the data format p. i =[θ i , l i ], where p i For the i-th data point, which is the distance l of the nearest object measured by the i-th line beam of the line laser sensor. i and the corresponding angle θ i When there is no object within the detection range of the line laser sensor, the distance l of the nearest object within the range of the line laser sensor is... i =0. In this example, if the wire harness sensor is divided into 200 wire harnesses, the collected data includes 200 data points, and the data structure is: [p1, p2, ... p 200 ].

[0071] S2: Convert the points detected in the coordinate system of each line laser sensor into position points in the coordinate system of the detection device with the detection device as the origin.

[0072] like Figure 4 As shown, in one embodiment of the present invention, the origin of the coordinate system of the device under test is set at the bottom of the robot, and a three-dimensional coordinate system is constructed, wherein the x-axis points towards the forward direction, the y-axis is horizontal, the positive y-axis points to the left, the z-axis is vertical, and the positive z-axis points directly upward.

[0073] The method for converting the points detected in the coordinate system of each line laser sensor into position points in the coordinate system of the detection device with the detection device as the origin in this example is as follows:

[0074] Data p from the line laser sensor i =[θ i , l i The corresponding point in the online laser coordinate system is: l p i =[l i cos(θ i ),l i sin(θ i ),0],

[0075] The position points converted to the coordinate system of the detection equipment are: in, l p i Let be the coordinates of the point detected by the i-th line laser sensor in the line laser sensor coordinate system. b p i Let be the coordinates of the point detected by the laser sensor along the i-th line in the coordinate system of the detection device. Let be the rotation matrix from the line laser sensor coordinate system to the detection device coordinate system. For example, the translation vector is used. Using a 3x3 rotation matrix, A 3*1 translation vector is used.

[0076] Step S3: Perform fall detection and low obstacle detection.

[0077] Step S301: Drop Detection

[0078] When a valid location point in the drop detection zone is detected, the difference between the distance the line laser should have hit the ground and the actual distance it hits the ground is calculated. When the difference exceeds a first set value, it is marked as a drop.

[0079] The specific processing method for determining whether the detection data of harness i indicates a drop is as follows:

[0080] (1) For harness i, calculate the ground that it should detect. ground p i ;

[0081] (2) The direction vector of beam i in the online laser coordinate system is: l n i =[cos(θ) i ),sin(θ i ),0] transformed into the direction vector in the coordinate system of the detection equipment b n i for: The point hit by the corresponding beam is: Where r is the length of the vector;

[0082] (3) By ground p i [2] = 0, therefore: in, ground p i [2] is the height of the ground hit by the wire harness. This is the last element of the 3*1 translation vector. b n i [2] is the last element of the 3*1 direction vector;

[0083] (4) Based on vector r, from the formula:

[0084]

[0085] Get the ground point that should be hit ground p i ;

[0086] (5) When l i =0 or |l i If -r is greater than the first set value, it is marked as a fall; otherwise, it is marked as no fall.

[0087] The initial setting in this example is 0.3m. This value is determined based on the actual situation, meaning that a 30cm elevation difference is considered a fall. It can be adjusted according to the actual situation.

[0088] In drop detection, when the difference exceeds a first preset value, it is marked as a potential fall. This also includes a sub-step for filtering the fall area. Normally, a fall is not detected by only a single laser sensor beam. Here, we assume that if a fall is occurring in front of the robot, several consecutive lines will show relatively high fall scores. Simultaneously, this strategy can also filter out false detections from the sensors.

[0089] like Figure 5 As shown, the processing method for the filtering of the fall area sub-step is as follows:

[0090] A1: Find the first bundle m marked as fallen;

[0091] A2: Find the next bundle n marked as fallen;

[0092] A3: Determine if the difference between n and m is greater than the third set value. If not, mark all wire harnesses m and n as not falling. If yes, proceed to the next step.

[0093] A4: Increase the total number of consecutive falls by 1;

[0094] A5: Determine if the total number of consecutive drops exceeds the threshold. If so, mark all wire harnesses mn as dropped.

[0095] In this example, before filtering the fall detection area, the range of the fall detection harness is set, where harness i ∈ [s]. cliff ,e cliff ];

[0096] The drop zone is set to start s = -1, end e = -1, and drop length l = 0.

[0097] Traverse the range and find the falling region using the filtering falling region sub-step method. The return form of the falling region is: [[i1,j1],[i2,j2],…].

[0098] Step S302: Obstacle Detection

[0099] like Figure 6 As shown, obstacle detection in this example is actually ground detection. If the ground height exceeds the height that the robot can cross, it is identified as an obstacle. The obstacle detection method in this example is as follows:

[0100] For the beam segmentation of the line laser sensor, if there is an initial ground, calculate the straightness error of each beam segment, and then calculate the error between the beam segment and the initial ground. If the sum of the straightness error and the error is less than a second set value, it is marked as ground; otherwise, it is marked as an obstacle.

[0101] Furthermore, in step S3, during the obstacle detection step, if there is no initial ground, all points detected by the segmented harness are marked as non-ground.

[0102] The method for segmenting the wire harness is as follows:

[0103] (1) The original setting is that the range of the detection ground wire bundle is i∈[s ground e ground ], where s ground =0,e ground Determined based on the obstacle-crossing capability of the detection equipment;

[0104] (2) Based on the detected drop areas, these drop areas are excluded from the ground detection area, resulting in the actual ground detection area.

[0105] (3) In each wire harness segment k, the actual ground detection area k∈[1, 2, ..., m], is split according to the set maximum length to obtain a new actual ground detection area. n represents the total number of new actual ground detection areas.

[0106] Furthermore, the initial ground detection method in this example is as follows:

[0107] Traverse each region The ground score for each region is calculated sequentially. When a set threshold is met, it indicates that the initial ground region has been found.

[0108] For each region, the method for calculating the ground score is as follows:

[0109] (1) Convert the points within the area to their positions in the coordinate system of the detection equipment;

[0110] (2) Use p to detect points at infinity. inf Fill in the blanks to obtain the sequence P = [ b p1,... b p k ], where k = ji, if the sequence P contains non-p inf If the number of points is less than the threshold, the area is determined to be a non-ground area.

[0111] (3) Extract the height of the position points in sequence P, and take the absolute value to obtain the difference P between the height and the ideal ground. z P z =[| b p1[2]|,| b p2[2]|,...,| b p k [2]|],where,| b p k [2]| represents the height of the k-th position;

[0112] (4) Statistical P z The mean E[P z ], and variance D[P z ],if If the value is less than the set value, it means that the initial ground has been found.

[0113] Furthermore, if the initial ground is detected, the method for obtaining the straight line of the initial ground is as follows:

[0114] (1) Regarding the initial ground The corresponding sequence P in the coordinate system of the detection equipment is: P = [ b p1,... b p k Extract the straight line from it;

[0115] (2) Transform sequence P into the loz two-dimensional coordinate system, where the loz two-dimensional coordinate system is a two-dimensional coordinate system composed of the l-axis and the z-axis, with the origin of the detection device as the origin, the l-axis is horizontal, and the z-axis is vertically upward. The l-axis represents the horizontal distance of the point from the robot origin, and the z-axis represents the vertical height.

[0116] pass z = b p k [2], the obtained sequence P loz for:

[0117] P loz =[ loz p1, loz p2, ..., loz p k ]

[0118] in, loz p k This is the reading of the k-th point in the sequence in the loz 2D coordinate system.

[0119] (3) Linear fitting:

[0120] Assuming the equation of the line is y = ux + v, then construct matrix A.

[0121]

[0122] Straightness is the coefficient of straightness. loz p1[0] is the first element of the reading of the first point in the sequence under the loz two-dimensional coordinate system. loz p1[1] is the second element of the reading of the first point in the sequence under the loz two-dimensional coordinate system.

[0123] Furthermore, after obtaining the initial straight line of the ground, the specific processing method for obstacle detection is as follows:

[0124] (1) Regarding the initial ground Then, for the ground detection area where k > m, the ground detection area of ​​the k-th wire bundle is... The corresponding sequence is

[0125] (2) Construct A, x, y for the ground detection area, and construct the straightness deviation e1 of the segmented beam, e1 = A(A T A)-A T yy and the difference e2 between the constructed and initialized ground straightness coefficients, in, The straightness coefficient of the initial ground surface. The coefficients of the fitted straight line for the ground detection area of ​​the wire harness k;

[0126] (3) If ||e1||2+||e2||2 is less than the second set value, such as 0.04, it means that the current detection area is all ground. Otherwise, the current detection area is not all ground. In this example, the second set value represents the height that needs to be regarded as a low obstacle, which is related to the robot's obstacle crossing ability. It can also be adjusted according to the actual situation.

[0127] (4) When the current detection area is not entirely the ground, then traverse the candidate detection areas. An inner point, whose coordinates are loz p k Find the point with the largest straight-line distance d from the candidate detection region. The distance d is calculated as follows: p[0] is the first element of each point, and p[1] is the second element of each point within the detection area;

[0128] (5) This detection area Divided into two regions Then, obstacle detection is performed until the entire ground is delineated.

[0129] Therefore, the algorithm described in this example can effectively detect all line points and distinguish between fallen or low obstacles. This invention, by vertically placing two or more line laser sensors in front of the device under test, can effectively fill blind spots using only line laser sensors. If more sensors are placed on the detection device, the customer can achieve a 360-degree detection range at a lower cost.

[0130] By segmenting the beam of the line laser sensor for detection, the ambiguity of the sensor data in slope scenarios can be eliminated. In addition, the detection method of this invention can effectively detect falls and low obstacles, improving the safety of autonomous operation of mobile detection equipment.

[0131] The specific embodiments described above are preferred embodiments of the present invention and are not intended to limit the specific scope of the present invention. The scope of the present invention includes, but is not limited to, these specific embodiments. All equivalent changes made in accordance with the present invention are within the protection scope of the present invention.

Claims

1. A method for detecting a fall and a low obstacle based on a solid-state line laser, characterized by, The detection device is provided with two or more line laser sensors, which are installed vertically, wherein at least two line laser sensors are arranged in front of the traveling direction, and the fall and low obstacle detection method based on the solid-state line laser comprises the following steps: S1: obtaining detection data of the line laser sensor; S2: converting the points detected in the line laser sensor coordinate system into position points in the detection device coordinate system with the detection device as the origin; S3: performing fall detection and low obstacle detection, wherein Fall detection: when an effective position point in the fall detection area is detected, the difference between the distance of the ground where the line laser should hit and the actual distance of the ground where the line laser hits is calculated, and when the difference exceeds the first set value, it is marked as a fall; Obstacle detection: the light beams of the line laser sensor are segmented, and if there is an initial ground, the straightness error of each segmented light beam is calculated, and then the error between the segmented light beam and the initial ground is calculated, and when the sum of the straightness error and the error is less than the second set value, it is marked as the ground, otherwise it is marked as an obstacle, In step S3, in the obstacle detection step, if there is no initial ground, all the points detected by the segmented light beams are marked as non-ground, The method for segmenting the light beams is: (1) The range of the wire bundle i for detecting the ground surface is originally set to wherein, , According to the obstacle surmounting ability of the detection device, i is the serial number of the laser sensor wire bundle; (2) According to the detected falling area, the falling area is excluded from the ground detection area, and the actual ground detection area is obtained as ; In each wire harness section Actual ground detection area , , according to the set maximum length split processing, get the new actual ground detection area , n is the total number of new actual ground detection area.

2. The solid-state line laser based drop and low obstacle detection method according to claim 1, characterized in that: The detection device is a robot that can travel, and the number of line laser sensors is two, which are respectively installed at the left and right sides of the front of the robot in the advancing direction.

3. The solid-state line laser-based drop and low obstacle detection method according to claim 1, characterized in that: In step S1, the points collected by the line laser sensor are in the form of a data set, and the data format is as follows: Where f represents the number of data points in each frame, and the data format of each point is as follows: ,in, For the first The data point, which is the first data point of the line laser sensor. The distance to the nearest object measured by the line harness. and the corresponding angle When there is no object within the detection range of the line laser sensor, the distance to the nearest object within the range of the line laser sensor is... .

4. The solid-state line laser-based drop and low obstacle detection method according to claim 3, characterized in that: In step S2, one point on the detection device is set as the origin of the detection device coordinate system, and a three-dimensional coordinate system is constructed, wherein the x-axis is directed to the advancing direction, the y-axis is horizontal, and the z-axis is vertical. The calculation method for converting the points detected in the line laser sensor coordinate system into position points in the detection device coordinate system with the detection device as the origin is: Data of the line laser sensor The corresponding point in the point of the line laser coordinate system is: , The position point converted into the coordinate system of the detection device is: wherein is the position point of the i-th line laser sensor in the coordinate system of the detection device, is the point coordinate of the point detected by the i-th line laser sensor in the coordinate system of the line laser sensor, is the position point of the i-th line laser sensor in the coordinate system of the detection device, is the point coordinate of the point detected by the i-th line laser sensor in the coordinate system of the line laser sensor, is the rotation matrix from the coordinate system of the line laser sensor to the coordinate system of the detection device, is the translation vector.

5. The solid-state line laser-based drop and low obstacle detection method according to claim 4, characterized in that: In step S3, in the fall detection, when the difference exceeds the first set value, it is marked as a fall, and further includes a fall area filtering sub-step, and the processing method of the fall area filtering sub-step is: A1: find the first line beam c marked as a fall; A2: find the next line beam d marked as a fall; A3: determine whether the difference between c and d is greater than a third set value, if not, mark all line beams c-d as not falling, if yes, execute the next step; A4: increase the total number of consecutive falls by 1; A5: determine whether the total number of consecutive falls is greater than a threshold, if yes, mark all line beams c-d as falling.

6. The solid-state line laser-based drop and low obstacle detection method according to claim 5, characterized in that: The specific processing method of the fall detection is: (1) For a wire bundle , calculate the ground points it should detect ; (2) wiring harness The directional vector in the online laser coordinate system is: , transforming to the direction vector in the coordinate system of the detection device is: , then the point hit by the corresponding light beam is: wherein is the length of the vector; (3) by It is available: where, is the height of the ground hit by the wire bundle, is the last element of the 3*1 translation vector, is the last element of the 3*1 direction vector; (4) According to the vector by the formula: get the point of the ground that should be hit ; (5) When or greater than a first set value, then marked as a fall, otherwise, marked as not falling.

7. The solid-state line laser-based drop and low obstacle detection method according to claim 1, characterized by, The detection method of the initial ground is: traversing the regions , the ground fraction of each region is calculated in turn, and when a set threshold is met, it is representative of finding an initial ground region, For each area, the method for calculating the ground score is: (1) converting the points in the area into position points in the detection device coordinate system; (2) Detection of points at infinity using filling, resulting in a sequence wherein, if the number of non ground points in the sequence is less than a threshold, the region is determined to be a non-ground region; (3) extraction sequence the height of the inside position point, and take the absolute value to obtain the difference from the ideal ground , wherein, is the height of the kth position point; (4) Statistics of the mean and variance if is less than a set value, then the initial ground is found.

8. The solid-state line laser-based drop and low obstacle detection method according to claim 7, characterized in that: After the initial ground is detected, the straight line of the initial ground is obtained in the following way: (1) For the initial ground , the sequence in the corresponding detection device coordinate system is : , and a straight line is extracted therefrom; (2) convert the sequence to a loz two-dimensional coordinate system with the origin at the origin of the coordinate system of the device under test, wherein , the resulting sequence : wherein, loz is the reading of the kth point in the sequence in the loz two-dimensional coordinate system; (3) straight line fitting: Assume the straight line equation is , then construct matrix A, , , Straightness is the coefficient of the straight line , is the first element of the first point reading in the sequence in the loz two-dimensional coordinate system, is the second element of the first point reading in the sequence in the loz two-dimensional coordinate system.

9. The solid-state laser-based drop-off and low- obstacle detection method according to claim 8, characterized in that: After the straight line of the initial ground is obtained, the specific processing method of the obstacle detection is: (1) For the initial ground , then determine ground detection area, the first ground detection area of the line bundle is , the corresponding sequence is ; (2) constructing A, x, y of the ground detection area, constructing the deviation of the straightness of the segmented light beam , , and constructing the difference from the initial ground straight line coefficient , , wherein is the initial ground straight line coefficient, is the coefficient of the ground detection area fitting line of the light beam k (3) If the value is less than the second set value, it means that the current detection area is all ground, otherwise the current detection area is not all ground. (4) When the current detection region is not all ground, then traverse the candidate detection region A point in the detection region, whose coordinates are Find the point in the candidate detection region that is farthest from the straight line d, and the calculation method of the distance d is: , The first element of each point, The second element of each point in the detection region; (5) The detection region is divided into two regions and the obstacle detection is performed again. and the obstacle detection is performed again.​

Images