Drop and low obstacle detection method based on solid-state line laser

Abstract

The invention provides a falling and low obstacle detection method based on solid-state line laser, and belongs to the technical field of robot application. The detection equipment is provided with more than two line laser sensors, the line laser sensors are vertically mounted, and the falling and low obstacle detection method based on the solid-state line laser comprises the following steps: acquiring detection data of the line laser sensors; converting points detected under each line laser sensor coordinate system into position points under a detection equipment coordinate system with detection equipment as an original point; and carrying out drop detection and low obstacle detection, segmenting the light beams of the line laser sensor, if an initial ground exists, calculating the straightness error of each segmented light beam, then calculating the error between the segmented light beams and the initial ground, and when the sum of the straightness error and the error is smaller than a second set value, marking as the ground, otherwise, marking as an obstacle. According to the invention, the data ambiguity of the sensor in the slope scene can be eliminated, and the safety of autonomous operation of equipment is improved.

Description

Technical Field

[0001] The present invention relates to the field of robot application technology, and in particular to a fall and low obstacle detection method based on solid-state line laser. Background Art

[0002] Indoor mobile robots generally rely on single-line laser radar, RGBD camera and other sensors to detect surrounding obstacles and avoid collisions during movement. Due to the limited FOV (field of view) of the sensor, it is generally unable to cover the 360° range around the robot. Some sweeping robots use solid-state line lasers for blind spots, but they are generally installed horizontally. Although this can maximize the coverage of the sensor, in complex scenes, such as when there is a slope in front of the robot, since all solid-state line lasers are arranged at a height, it is impossible to achieve mutual detection at multiple angles, and often misdetection problems occur. Summary of the invention

[0003] In order to solve the problems in the prior art, the present invention provides a method for detecting falls and low obstacles based on solid-state line laser.

[0004] The present invention is based on a solid-state line laser fall and low obstacle detection method, the detection equipment is provided with two or more line laser sensors, the line laser sensors are vertically installed, the solid-state line laser fall and low obstacle detection method based on the laser comprises the following steps:

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

[0006] S2: 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;

[0007] S3: Perform fall detection and low obstacle detection, where:

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

[0009] Obstacle detection: The beam of the line laser sensor is segmented. If there is an initial ground, the straightness error of each segmented beam is calculated, and then the error between the segmented beam and the initial ground is calculated. 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.

[0010] Furthermore, the detection device is a robot capable of walking, and the number of the line laser sensors is two, which are respectively installed at set positions on the left and right sides in front of the robot's forward direction.

[0011] Furthermore, in step S1, the points collected by the line laser sensor are in the form of a data set, and the data form is: [p 1 , p 2 , ..., p f ], where f represents the number of points in each frame of data, and the data format of each point is p i =[θ i , l i ], where p i is the i-th data point, that 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 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 to construct a three-dimensional coordinate system, wherein the x-axis is in the forward direction, the y-axis is in the horizontal direction, and the z-axis is in the vertical direction. The calculation method for converting the points detected in the coordinate system of each line laser sensor into the position points in the detection device coordinate system with the detection device as the origin is:

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

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

[0015] Further, in step S3, during the fall detection, when the difference exceeds the first set value, it is marked as before the fall, and further includes a sub-step of filtering the fall area. The processing method of the sub-step of filtering the fall area is:

[0016] A1: Find the first harness m marked as dropped;

[0017] A2: Find the next harness n marked as dropped;

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

[0019] A4: The total number of consecutive falls plus 1;

[0020] A5: Determine whether the total number of consecutive drops is greater than a threshold. If so, mark all harnesses mn as drops.

[0021] Furthermore, the specific processing method of the fall detection is:

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

[0023] (2) The direction vector of beam i in the linear laser coordinate system is: l n i =[cos(θ i ), sin(θ i ),0] converted to the direction vector in the detection device coordinate system b n i for: Then the point where the corresponding light beam hits is: Where r is the length of the vector;

[0024] (3) By ground p i [2] = 0, we get: in, ground p i [2] is the height of the ground where the harness hits, 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) According to the vector r, the formula:

[0026]

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

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

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

[0030] The method of segmenting the wiring harness is as follows:

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

[0032] (2) According to the detected falling areas, these falling areas are excluded from the ground detection area, and the actual ground detection area is obtained as follows:

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

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

[0035] Traveling through various areas The ground score of each area is calculated in turn. When the set threshold is met, it means that the initial ground area is found.

[0036] For each region, the ground score is calculated as:

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

[0038] (2) Detecting a point at infinity using p inf Fill, and get the sequence P = [ b p 1 ,… b p k ], where k = ji, if the non-p in the sequence P inf If the number of points is less than the threshold, the area is judged as a non-ground area;

[0039] (3) Extract the height of the position point in the sequence P and take the absolute value to obtain the difference P from the ideal ground. z , P z =[ b p 1 [2]],| b p 2 [2]|,...,|b p k [2]|], where | b p k [2]| is the height of the kth position point;

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

[0041] Furthermore, the initial ground is detected, and the straight line of the initial ground is obtained as follows:

[0042] (1) For the initial ground The corresponding sequence P in the detection equipment coordinate system is: P = [ b p 1 ,... b p k ], and extract it in straight lines;

[0043] (2) The sequence P is transformed into the loz two-dimensional 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 p 1 , loz p 2 , ..., loz p k ]

[0045] in, loz p k is the reading of the kth point in the sequence in the loz two-dimensional coordinate system.

[0046] (3) Linear fitting:

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

[0048] Furthermore, after the initial straight line of the ground is obtained, the specific processing method of the obstacle detection is:

[0049] (1) For the initial ground Then the ground detection area of ​​k>m is determined, and the ground detection area of ​​the kth harness is The corresponding sequence is

[0050] (2) Construct A, x, and y for the ground detection area and construct the straightness deviation e of the segmented beam 1 , e 1 =A(A T A) -1 A T yy, and the difference between the constructed and initialized ground straight line coefficients e 2 , in, is the linear coefficient of the initial ground, is the coefficient of the straight line fitting of the ground detection area of ​​the harness k;

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

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

[0053] (5) The detection area Split into two areas Then the obstacle detection is performed.

[0054] Compared with the prior art, the beneficial effects of the present invention are: more than two line laser sensors are arranged in the vertical direction in front of the device to be tested. The use of line laser sensors alone can take into account blind spot filling. If more line laser sensors are arranged on the detection device, the customer can achieve a 360-degree detection range at a lower cost.

[0055] By using segmented beam detection of the line laser sensor, data ambiguity of the sensor in slope scenarios can be eliminated. In addition, the detection method of the present invention can well detect falls and low obstacles, thereby improving the safety of autonomous operation of the mobile detection equipment. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] In order to more clearly illustrate the present invention or the solutions in the prior art, a brief introduction is given below to the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0057] Figure 1 This is a schematic diagram of the installation of a line laser sensor for a detection device according to an embodiment of the present invention;

[0058] Figure 2 Schematic diagram of the detection range of the detection device of the present invention;

[0059] Figure 3 It is a flow chart of the detection method of the present invention;

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

[0061] Figure 5 This is a flow chart of a method for fall detection according to an embodiment;

[0062] Figure 6 The figure is a flow chart of a method for obstacle detection according to an embodiment. DETAILED DESCRIPTION

[0063] Unless otherwise defined, all technical and scientific terms used in the present invention have the same meanings as those commonly understood by those skilled in the art to which the present invention belongs; the terms used in the specification of the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention; the terms "including" and "having" and any variations thereof in the specification and claims of the present invention and the above-mentioned drawings are intended to cover non-exclusive inclusions. The terms "first", "second", etc. in the specification and claims of the present invention or the above-mentioned drawings are used to distinguish different objects, not to describe a specific order.

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

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

[0066] Movable detection equipment, such as indoor mobile robots, often face more than just flat ground. Some small slopes and small objects dropped on the ground are also very common. In order to achieve automated operation in complex indoor scenes, multiple sensors are needed to cover the robot's 360° detection range. Although the solid-state line laser has a limited detection distance, its cost is relatively low. This example uses an indoor mobile robot and the minimum number of line laser sensors as an example to explain. Of course, if the number of line laser sensors is increased and multiple sensors are used, the robot's 360° detection range can be covered.

[0067] like Figure 1 and Figure 2 As shown, in the present invention, a line laser sensor 2, 3 is vertically installed at designated positions on both sides of the 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, 3 to expand the coverage range in front, and the detection results of the two line laser sensors 2, 3 can verify each other, thereby improving the accuracy of detection and taking into account blind spot filling.

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

[0069] Step S1: Acquire detection data of the line laser sensor.

[0070] In this step, the points collected by the line laser sensor are in the form of a data set, and the data form is: [p 1 , p 2 , ..., p f ], where f represents the number of points in each frame of data, and the data format of each point is p i =[θ i , l i ], where p i is the i-th data point, that 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 i= 0. In this example, if the wire harness sensor is divided into 200 wire harnesses, the collected data includes 200 point data, and the data behavior is: [p 1 , p 2 , ...p 200 ].

[0071] S2: 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.

[0072] like Figure 4 As shown, as an embodiment of the present invention, this example sets the origin of the coordinate system of the device to be tested at the bottom of the robot, and constructs a three-dimensional coordinate system, wherein the x-axis is in the forward direction, the y-axis is in the horizontal direction, wherein the positive y-axis direction is toward the left, and the z-axis is in the vertical direction, wherein the positive z-axis direction is toward directly upwards.

[0073] In this example, the calculation method for converting the points detected in the line laser sensor coordinate system into the position points in the detection device coordinate system with the detection device as the origin is:

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

[0075] The position point converted to the detection equipment coordinate system is: in, l p i is the point coordinates of the point detected by the i-th line laser sensor in the line laser sensor coordinate system, b p i is the position coordinate of the point detected by the laser sensor on the i-th line in the coordinate system of the detection device, is the rotation matrix from the line laser sensor coordinate system to the detection device coordinate system, is the translation vector, in this case Using a 3*3 rotation matrix, Use a 3*1 translation vector.

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

[0077] Step S301: Drop detection

[0078] When a valid position point of the drop detection zone is detected, the difference between the ground distance that the line laser should reach and the ground distance actually reached is calculated, and when the difference exceeds a first set value, it is marked as a drop.

[0079] Calculate whether the detection data of harness i is a drop. The specific processing method is:

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

[0081] (2) The direction vector of beam i in the linear laser coordinate system is: l n i =[cos(θ i ), sin(θ i ),0] converted to the direction vector in the detection device coordinate system b n i for: Then the point where the corresponding light beam hits is: Where r is the length of the vector;

[0082] (3) By ground p i [2] = 0, we get: in, ground p i [2] is the height of the ground where the harness hits, 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) According to the vector r, the formula:

[0084]

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

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

[0087] The first setting value in this example is 0.3m. This value is set according to the actual situation, which means that a 30CM elevation difference is regarded as a fall, and it can be adjusted according to the actual situation.

[0088] In the fall detection, when the difference exceeds the first set value, it is marked as before the fall, and the filter fall area sub-step is also included. Normally, a fall is not detected by only one line of laser sensor harness. Here, if there is a fall in front of the robot, there will be several consecutive lines with relatively high fall scores. At the same time, such a strategy can also filter out false detections of sensors.

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

[0090] A1: Find the first harness m marked as dropped;

[0091] A2: Find the next harness n marked as dropped;

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

[0093] A4: The total number of consecutive falls plus 1;

[0094] A5: Determine whether the total number of consecutive drops is greater than a threshold. If so, mark all harnesses mn as drops.

[0095] In this example, before filtering the drop area, the range of the wire harness for detecting the drop is set, and the wire harness i∈[s cliff ,e cliff ];

[0096] And set the drop area start s = -1, end e = -1, drop length l = 0;

[0097] Traverse the range and find the drop area according to the filter drop area sub-step method. The return form of the drop area is: [[i 1 ,j 1 ],[i 2 ,j 2 ],…].

[0098] Step S302: Obstacle detection

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

[0100] For the beam segmentation of the line laser sensor, if there is an initial ground, the straightness error of each segmented beam is calculated, and then the error between the segmented beam and the initial ground is calculated. 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.

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

[0102] The method of segmenting the wiring harness is as follows:

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

[0104] (2) According to the detected falling areas, these falling areas are excluded from the ground detection area, and the actual ground detection area is obtained as follows:

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

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

[0107] Traveling through various areas The ground score of each area is calculated in turn. When the set threshold is met, it means that the initial ground area is found.

[0108] For each region, the ground score is calculated as:

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

[0110] (2) Detecting a point at infinity using p inf Fill, and get the sequence P = [ b p 1 ,... b p k ], where k = ji, if the non-p in the sequence P inf If the number of points is less than the threshold, the area is judged as a non-ground area;

[0111] (3) Extract the height of the position point in the sequence P and take the absolute value to obtain the difference P from the ideal ground. z , P z =[| b p 1 [2]|,| b p 2 [2]|, ..., | b p k[2]|], where | b p k [2]| is the height of the kth position point;

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

[0113] Furthermore, the initial ground is detected, and the straight line of the initial ground is obtained as follows:

[0114] (1) For the initial ground The corresponding sequence P in the detection equipment coordinate system is: P = [ b p 1 ,... b p k ], and extract it in straight lines;

[0115] (2) The sequence P is converted into the loz two-dimensional coordinate system, where loz is a two-dimensional coordinate system consisting 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 vertical. 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 p 1 , loz p 2 , ..., loz p k ]

[0118] in, loz p k is the reading of the kth point in the sequence in the loz two-dimensional coordinate system.

[0119] (3) Linear fitting:

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

[0121]

[0122] Straightness is the coefficient of the straight line. loz p 1[0] is the first element of the first point reading in the sequence in the loz two-dimensional coordinate system, loz p 1 [1] is the second element of the first point reading in the sequence in the loz two-dimensional coordinate system.

[0123] Furthermore, after the initial straight line of the ground is obtained, the specific processing method of the obstacle detection is:

[0124] (1) For the initial ground Then the ground detection area of ​​k>m is determined, and the ground detection area of ​​the kth harness is The corresponding sequence is

[0125] (2) Construct A, x, and y for the ground detection area and construct the straightness deviation e of the segmented beam 1 , e 1 =A(A T A)-A T yy and the difference between the constructed and initialized ground straight line coefficients e 2 , in, is the linear coefficient of the initial ground, is the coefficient of the straight line fitting of the ground detection area of ​​the harness k;

[0126] (3)||e 1 || 2 +||e 2 || 2 When it is less than the second setting 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 setting value represents the height of the obstacle to be considered as low, which is related to the robot's obstacle crossing ability and can be adjusted according to the actual situation.

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

[0128] (5) The detection area Split into two areas Then, obstacles are detected until the entire ground is divided.

[0129] Therefore, through the above algorithm of this example, all harness points can be effectively detected, and falls or low obstacles can be distinguished. The present invention sets more than two line laser sensors in the vertical direction in front of the device to be tested. The use of line laser sensors alone can take into account blind spots. If more number of line laser sensors are set on the detection equipment, the customer can achieve a 360-degree detection range at a lower cost.

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

[0131] The specific implementation modes described above are preferred implementation modes of the present invention, and are not intended to limit the specific implementation scope of the present invention. The scope of the present invention includes but is not limited to the specific implementation modes, and all equivalent changes made according to the present invention are within the protection scope of the present invention.

Claims

1. A method for detecting falls and low obstacles based on solid-state line laser, characterized in that: The detection equipment is provided with more than two line laser sensors, which are installed vertically, wherein at least two line laser sensors are provided in front of the traveling direction, and the fall and low obstacle detection method based on solid-state line laser comprises the following steps: S1: Acquire detection data of the line laser sensor; S2: 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; S3: Perform fall detection and low obstacle detection, where: Drop detection: When a valid position point in the drop detection area is detected, the difference between the ground distance that the line laser should hit and the actual ground distance is calculated. When the difference exceeds the first set value, it is marked as a drop; Obstacle detection: The beam of the line laser sensor is segmented. If there is an initial ground, the straightness error of each segmented beam is calculated, and then the error between the segmented beam and the initial ground is calculated. 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.

2. The method for detecting falling objects and low obstacles based on solid-state line laser according to claim 1, characterized in that: The detection device is a robot capable of walking, and the number of the line laser sensors is two, which are respectively installed at set positions on the left and right sides in front of the robot's forward direction.

3. The method for detecting falling objects and low obstacles based on solid-state line laser 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 form is: [p1, p2, ..., p f ], where f represents the number of points in each frame of data, and the data format of each point is p i =[θ i , l i ], where p i is the i-th data point, that 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 i =0.

4. The method for detecting falling objects and low obstacles based on solid-state line laser according to claim 3, characterized in that: In step S2, one of the points on the detection device is set as the origin of the detection device coordinate system to construct a three-dimensional coordinate system, in which the x-axis is in the forward direction, the y-axis is in the horizontal direction, and the z-axis is in the vertical direction. The calculation method for converting the points detected in the coordinate system of each line laser sensor into the position points in the detection device coordinate system with the detection device as the origin is: Data of line laser sensor i =[θ i , l i ]The corresponding point in the line laser coordinate system is: l p i =[l i cos(θ i ), l i sin(θ i ),0], The position point converted to the detection equipment coordinate system is: in, l p i is the point coordinates of the point detected by the i-th line laser sensor in the line laser sensor coordinate system, b p i is the position coordinate of the point detected by the laser sensor on the i-th line in the coordinate system of the detection device, is the rotation matrix from the line laser sensor coordinate system to the detection device coordinate system, is the translation vector.

5. The method for detecting falling objects and low obstacles based on solid-state line laser according to claim 4, characterized in that: In step S3, during the fall detection, when the difference exceeds the first set value, it is marked as before the fall, and further includes a sub-step of filtering the fall area. The processing method of the sub-step of filtering the fall area is: A1: Find the first harness m marked as dropped; A2: Find the next harness n marked as dropped; A3: Determine whether the difference between n and m is greater than a third set value. If not, mark all the harnesses mn as not falling. If yes, proceed to the next step. A4: The total number of consecutive falls plus 1; A5: Determine whether the total number of consecutive drops is greater than a threshold. If so, mark all harnesses mn as drops.

6. The method for detecting falling objects and low obstacles based on solid-state line laser according to claim 5, characterized in that: The specific processing method of the fall detection is: (1) For harness i, calculate the ground it should detect ground p i ; (2) The direction vector of beam i in the linear laser coordinate system is: l n i =[cos(θ i ), sin(θ i ), 0], converted to the direction vector in the detection device coordinate system b n i for: Then the point where the corresponding light beam hits is: Where r is the length of the vector; (3) By ground p i [2] = 0, we get: in, ground p i [2] is the height of the ground where the harness hits, is the last element of the 3*1 translation vector, b n i [2] is the last element of the 3*1 direction vector; (4) According to the vector r, the formula: Get the ground point that should be hit ground p i ; (5) When l i =0 or |l i -r| is greater than the first set value, it is marked as a drop, otherwise, it is marked as no drop.

7. The method for detecting falling objects and low obstacles based on solid-state line laser according to claim 5, characterized in that: In step S3, in the obstacle detection step, if there is no initial ground, all the points detected by the segmented harness are marked as non-ground. The method of segmenting the wiring harness is as follows: (1) The original setting of the detection ground wire range is i∈[s ground , e ground ], where s ground =0,e ground Determined according to the obstacle-crossing capability of the detection equipment; (2) According to the detected falling areas, these falling areas are excluded from the ground detection area, and the actual ground detection area is obtained as follows: (3) Actual ground detection area in each harness segment k k∈[1, 2, ..., m], split according to the set maximum length to obtain a new actual ground detection area n is the total number of new ground truth detection areas.

8. The method for detecting falling objects and low obstacles based on solid-state line laser according to claim 7, characterized in that: The initial ground detection method is: Traveling through various areas The ground score of each area is calculated in turn. When the set threshold is met, it means that the initial ground area is found. For each region, the ground score is calculated as: (1) Convert the points in the area to the position points in the coordinate system of the detection equipment; (2) Detecting a point at infinity using p inf Fill, and get the sequence P = [ b p1,... b p k ], where k = ji, if the non-p in the sequence P inf If the number of points is less than the threshold, the area is judged as a non-ground area; (3) Extract the height of the position point in the sequence P and take the absolute value to obtain the difference P from the ideal ground. z , P z =[| b p1[2]|,| b p2[2]|,...,| b p k [2]|], where | b p k [2]| is the height of the kth position point; (4) Statistics P z The mean value E[P z ], and the variance D[P z ],if If it is smaller than the set value, it means the initial ground has been found.

9. The method for detecting falling objects and low obstacles based on solid-state line laser according to claim 8, characterized in that: Once the initial ground is detected, the straight line of the initial ground is obtained as follows: (1) For the initial ground The corresponding sequence P in the detection equipment coordinate system is: P = [ b p1,... b p k ], and extract it in straight lines; (2) The sequence P is transformed into the loz two-dimensional 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: P loz =[ loz p1, loz p2,..., loz p k ] in, loz p k is the reading of the kth point in the sequence in the loz two-dimensional coordinate system. (3) Linear fitting: Assuming that the equation of the line is y=ux+v, construct the matrix A. Straightness is the coefficient of the straight line. loz p1[0] is the first element of the first point reading in the sequence in the loz two-dimensional coordinate system. loz p1[1] is the second element of the first point reading in the sequence in the loz two-dimensional coordinate system.

10. The method for detecting falling objects and low obstacles based on solid-state line laser according to claim 9, characterized in that: After the initial straight line of the ground is obtained, the specific processing method of the obstacle detection is as follows: (1) For the initial ground Then the ground detection area of ​​k>m is determined, and the ground detection area of ​​the kth harness is The corresponding sequence is (2) Construct A, x, y for the ground detection area and construct the straightness deviation e1 of the segmented light beam, e1 = A(A T A) - 1 A T yy, and the difference between the constructed and initialized ground straight line coefficients e2, in, is the linear coefficient of the initial ground, is the coefficient of the straight line fitting of the ground detection area of ​​the harness k; (3) When ||e1||2+||e2||2 is less than the second set value, it means that the current detection area is entirely the ground, otherwise the current detection area is not entirely the ground; (4) When the current detection area is not entirely ground, traverse the candidate detection areas A point inside, whose coordinates are loz p k , find the point with the largest straight-line distance d within the candidate detection area. The distance d is calculated as: p[0] is the first element of each point, and p[1] is the second element of each point in the detection area; (5) The detection area Split into two areas Then the obstacle detection is performed.

Images