CN114200472A - Real-time detection method and device for passable area, terminal and storage medium - Google Patents
Real-time detection method and device for passable area, terminal and storage medium Download PDFInfo
- Publication number
- CN114200472A CN114200472A CN202111346188.1A CN202111346188A CN114200472A CN 114200472 A CN114200472 A CN 114200472A CN 202111346188 A CN202111346188 A CN 202111346188A CN 114200472 A CN114200472 A CN 114200472A
- Authority
- CN
- China
- Prior art keywords
- elevator
- point
- preset
- robot
- cloud data
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/495—Counter-measures or counter-counter-measures using electronic or electro-optical means
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a real-time detection method, a real-time detection device, a real-time detection terminal and a real-time detection storage medium for passable areas, wherein point cloud data are acquired by emitting laser rays through a single-line laser radar, and are processed to generate a point set; judging the opening and closing state of the elevator door according to the comparison of the number of points meeting specific conditions in the point set and a preset first threshold value; generating a second preset number of line segments according to the acquired first preset number of corner point coordinates, and moving the line segments to enable the line segments to form a plurality of rectangular areas; and traversing a plurality of rectangular areas according to the point set, comparing the number of points falling in a single rectangular area with a preset second threshold value, and judging whether the robot can enter the elevator. Compared with the prior art, the method and the device have the advantages that the point cloud data are collected and processed, the elevator car passable area is detected from the two aspects of the state of the elevator door and whether the robot can enter the elevator, the detection efficiency and the accuracy of the detection result are improved, and the passing safety of the robot is ensured.
Description
Technical Field
The invention relates to the technical field of robot detection, in particular to a real-time detection method, a real-time detection device, a real-time detection terminal and a real-time detection storage medium for passable areas.
Background
Technological progress has led to rapid development of robotics, and in addition to the application of robotics in industrial manufacturing and other scenarios, the demand of service robots in hospitals, hotels, restaurants, airports, stations and other public places has also increased in a blowout manner. When the robot is to perform a cross-floor task, it is required to board the elevator autonomously. Under the condition that the space in the elevator cage is not clear, the surrounding environment of the robot is not judged, the robot enters the elevator in a faked manner, the robot is easily stopped at the elevator opening or only enters a part of the machine body, and the efficiency and the experience are influenced.
In the prior art, a multi-line laser radar is generally adopted to detect a three-dimensional space, the cost is high, the required calculation capacity is high, a large amount of system resources are occupied, and the algorithm is complex. The mobile robot detects the information of characteristic points from the mobile robot to the surrounding environment by using a laser ranging sensor; calculating the area of the whole space of the elevator; counting whether the number of the laser ray lengths larger than 2m is smaller than a specified threshold value T or not, if yes, the elevator can be closed, if the number of the laser ray lengths larger than 2m is larger than the threshold value T, the elevator door is in an open or open state, and the space area value of the elevator in the visual field which can be seen by the laser rays is counted; and (4) counting that the space area value of the elevator in the visual field which can be seen by the laser ray is larger than a preset value, and enabling the mobile robot to enter the elevator. In the prior art, whether the robot enters the elevator or not is judged by calculating the space area value of the elevator in the visual field which can be seen by laser rays, the method is complex in calculation, and the final area contains an invalid area, so that the detection efficiency is low, the accuracy of the detection result is not high, and the robot is easily influenced by the external environment.
Disclosure of Invention
The technical problem to be solved by the invention is as follows: the method, the device, the terminal and the storage medium for detecting the passable area in real time are provided, and the efficiency of detection and the accuracy of detection results are improved and the passing safety of the robot is ensured by collecting point cloud data and counting the point cloud data in a specific area.
In order to solve the technical problem, the invention provides a real-time detection method of a passable area, which comprises the following steps:
acquiring coordinates of elevator waiting points to enable the robot to navigate to the elevator waiting points, and calculating a first preset number of angular point coordinates formed by a single line laser radar scanning plane and an elevator through system parameters;
acquiring point cloud data by emitting laser rays through a single-line laser radar, and processing the point cloud data to generate a point set;
acquiring the abscissa corresponding to each point in the point set, comparing the number of points of which the numerical value is greater than the numerical value of the abscissa of a preset angular point with a preset first threshold value, and judging the opening and closing condition of the elevator door;
generating a second preset number of line segments according to the acquired first preset number of corner point coordinates, and moving the line segments to enable the line segments to form a plurality of rectangular areas with preset sizes;
and traversing the point set, counting the number of points falling in each rectangular area, comparing the number of points in each rectangular area with a preset second threshold value, and judging whether the robot enters the elevator.
Further, a laser ray is emitted through a single-line laser radar to collect point cloud data, the point cloud data is processed, and a point set is generated, specifically:
acquiring distance values detected by a plurality of laser rays by emitting the laser rays, screening the distance values, and acquiring original point cloud data corresponding to the screened distance values;
performing coordinate conversion on the original point cloud data, and performing voxel down-sampling and Euclidean clustering processing on the original point cloud data after the coordinate conversion;
and simultaneously, generating a point set corresponding to the processed original point cloud data according to the transformation relation of the coordinate system.
Further, generating a second preset number of line segments, and moving the line segments to form a plurality of rectangular areas with preset sizes, specifically:
virtually constructing a straight line in the elevator so that the distance from the straight line to an elevator door is a preset first preset distance, wherein the first preset distance is larger than the diameter of the robot;
and dividing the straight line into a second preset number of line segments, and moving the line segments back and forth to form a plurality of rectangular areas with preset sizes.
Further, the system parameters comprise the length and width of the elevator, the radius of the robot and the distance from the elevator waiting point to the elevator.
Further, the present invention also provides a real-time detection device for passable areas, comprising: the system comprises an acquisition module, a data processing module, a first judgment module, a region division module and a second judgment module, and specifically comprises:
the acquisition module is used for acquiring coordinates of the elevator waiting points so that the robot can navigate to the elevator waiting points, and calculating a first preset number of angular point coordinates formed by a single line laser radar scanning plane and the elevator according to system parameters;
the data processing module is used for acquiring point cloud data by emitting laser rays through a single-line laser radar, processing the point cloud data and generating a point set;
the first judgment module is used for acquiring the abscissa corresponding to each point in the point set, comparing the number of points with the abscissa value larger than the numerical value of the abscissa of the preset angular point with a preset first threshold value, and judging the opening and closing condition of the elevator door;
the region dividing module is used for generating line segments of a second preset number according to the acquired corner coordinates of the first preset number, and moving the line segments to enable the line segments to form a plurality of rectangular regions of preset sizes;
the second judging module is used for traversing the point set, counting the number of points falling in each rectangular area, comparing the number of points in each rectangular area with a preset second threshold value, and judging whether the robot enters the elevator.
Further, the data processing module is used for collecting point cloud data by emitting laser rays through a single-line laser radar, processing the point cloud data and generating a point set, and specifically comprises the following steps:
acquiring distance values detected by a plurality of laser rays by emitting the laser rays, screening the distance values, and acquiring original point cloud data corresponding to the screened distance values;
performing coordinate conversion on the original point cloud data, and performing voxel down-sampling and Euclidean clustering processing on the original point cloud data after the coordinate conversion;
and simultaneously, generating a point set corresponding to the processed original point cloud data according to the transformation relation of the coordinate system.
Further, the region dividing module is configured to generate a second preset number of line segments, and move the line segments so that the line segments form a plurality of rectangular regions of preset sizes, specifically:
virtually constructing a straight line in the elevator so that the distance from the straight line to an elevator door is a preset first preset distance, wherein the first preset distance is larger than the diameter of the robot;
and dividing the straight line into a second preset number of line segments, and moving the line segments back and forth to form a plurality of rectangular areas with preset sizes.
Further, the system parameters in the acquisition module comprise the length and width of the elevator, the radius of the robot and the distance from the elevator waiting point to the elevator.
Further, the present invention also provides a terminal device, which includes a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, and the processor implements the real-time detection method of the passable area according to any one of the above items when executing the computer program.
Further, the present invention also provides a computer-readable storage medium, which includes a stored computer program, wherein when the computer program runs, the apparatus where the computer-readable storage medium is located is controlled to execute the real-time detection method of a passable area as described in any one of the above.
Compared with the prior art, the real-time detection method, the real-time detection device, the real-time detection terminal and the real-time detection storage medium for the passable area have the following beneficial effects:
based on the characteristics of high stability, high precision, small data volume, small environmental influence and simple algorithm, the robot can obtain the point cloud data in the elevator through laser detection after reaching the formulated elevator waiting point through the autonomous navigation system, process the point cloud data to generate a point set, and judge the state of the elevator door through the number of points meeting specific conditions in the detection point set. After the opening and closing state of the elevator door is judged, in order to guarantee the passing safety of the robot, the area in the elevator is further divided into a plurality of rectangles, the point cloud data in the rectangles are further counted, and whether the robot enters the elevator is judged. Compared with the prior art, the method and the device have the advantages that the point cloud data are collected and processed, the elevator car passable area is detected from the two aspects of the state of the elevator door and whether the robot can enter the elevator, the detection efficiency and the accuracy of the detection result are improved, and the passing safety of the robot is ensured.
Drawings
FIG. 1 is a schematic flow chart diagram illustrating an embodiment of a method for real-time detection of a passable area according to the present invention;
FIG. 2 is a schematic structural diagram of an embodiment of a real-time detection apparatus for passable areas according to the present invention;
fig. 3 is a schematic view of four corner points in an elevator according to an embodiment of the method for real-time detection of passable areas according to the present invention;
fig. 4 is a schematic diagram of a rectangular area of a passable area in an embodiment of a real-time detection method of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the present application, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Example 1
Referring to fig. 1, fig. 1 is a schematic flow chart of an embodiment of a real-time detection method for a passable area according to the present invention, as shown in fig. 1, the method includes steps 101 to 105, which are specifically as follows:
step 101: acquiring coordinates of elevator waiting points to enable the robot to navigate to the elevator waiting points, and calculating a first preset number of angular point coordinates formed by a single line laser radar scanning plane and an elevator through system parameters;
in the embodiment, a 2D map is scanned and established by a single-line laser radar through a two-dimensional laser SLAM mapping algorithm, so that a unified map coordinate system is generated conveniently, the positions of the robot and the elevator are accurately obtained, the robot navigates to a waiting point P (x, y, z) in front of the elevator according to the obtained coordinates of the waiting point of the elevator based on a self-positioning navigation system, and the position of the robot at the waiting point is just opposite to the elevator door by adjusting the posture. In this embodiment, the robot adopts the right-hand coordinate system, and after the robot navigates to the elevator waiting point, it is assumed that the direction of the right-hand coordinate system of the robot is consistent with the direction of the map coordinate system, and if the direction of the coordinate system in the right hand of the robot is inconsistent with the direction of the map coordinate system, the direction of the robot can be adjusted according to the posture adjustment system. As a preferable solution in this embodiment, the first preset number is set to 4, and similarly, the first preset number may be adjusted according to actual situations. In this embodiment, as shown in fig. 3, the system parameters include the length L and width W of the elevator, the radius r of the robot, and the distance d from the elevator waiting point to the elevator; according to system parameters, coordinates of four corner points of the elevator on a single-line laser radar scanning surface relative to coordinates under map are respectively A (x + D, y + W/2, z), B (x + D + L, y + W/2, z), C (x + D, y-W/2, z) and D (x + D + L, y-W/2, z).
Step 102: and acquiring point cloud data by emitting laser rays through a single-line laser radar, and processing the point cloud data to generate a point set.
In the embodiment, the robot transmits a plurality of laser rays through a single line laser radar in a visual angle of a left-right symmetrical deviation of the elevator at a certain angle, obtains a distance value of the plurality of laser rays reflected back from an object, screens the distance value, keeps the distance value obtained by the laser rays transmitted from the visual angle of the robot right facing the left 90 degrees of the elevator door to the visual angle of the robot right facing the right 90 degrees of the elevator door, and obtains original point cloud data corresponding to the distance value kept after screening, wherein the original point cloud data mainly comprises polar coordinates of a plurality of point clouds. In this embodiment, adopt the mode of single line laser radar transmission laser, it is little influenced by the environment, and the testing result is more stable high-efficient, and the single line laser radar price is low, can further reduce the detection cost.
In the embodiment, the acquired point cloud data has the characteristics of high stability and high precision, the point cloud data volume is small, the influence of the environment is low, the algorithm is simple, the processing is convenient, the installation position of the single-line laser radar can be adjusted, the method is used for detecting elevator cars with different heights, and the application range is wide.
In this embodiment, the polar coordinates (ρ, θ) of the point cloud corresponding to the original point cloud data are converted into rectangular coordinates (x, y) according to a coordinate conversion formula, where the coordinate conversion formula is as follows:
in the implementation, the point cloud after coordinate conversion is subjected to voxel down-sampling and Euclidean clustering processing respectively, so that the purposes of removing noise points and reserving effective detection objects are achieved; and simultaneously, obtaining a transformational relation from a radar coordinate system to a map coordinate system by obtaining a tf tree of the robot, and converting the point cloud subjected to voxel downsampling and Euclidean clustering into a point set Q under the map coordinate system according to the transformational relation.
Step 103: and acquiring the abscissa corresponding to each point in the point set, comparing the number of points of which the numerical value is greater than the numerical value of the abscissa of the preset angular point with a preset first threshold value, and judging the opening and closing condition of the elevator door.
In the embodiment, traversing the point set, obtaining a numerical value i of an abscissa corresponding to each point in the point set, and comparing the numerical value i of the abscissa of the current point with a numerical value of an abscissa of a preset corner point based on the positioning deviation and the thickness of the elevator door, wherein the preset corner point is a point a, the numerical value of the abscissa of the point a is x + r + d + K, if the numerical value i of the abscissa of the current point is greater than the numerical value of the abscissa of the point a, the current point is considered to be a point falling in the elevator, counting the number of points in which the numerical value i of the abscissa of the point set is greater than the numerical value of the abscissa of the point a, if the number of points in which the numerical value i of the abscissa of the point set is greater than the numerical value of the abscissa of the point a is less than a preset first threshold value, judging that the elevator door is in an incompletely opened state, if the number of points in which the numerical value i of the abscissa of the point set is greater than the numerical value of the abscissa of the point a is greater than the preset first threshold value, is greater than or equal to the preset first threshold value, it is judged that the elevator door is opened.
In this embodiment, the abscissa of the point set is directly obtained to determine and reduce the calculation amount of the distance from the point to the point, the calculation speed is increased, and the preset allowable error K and the preset first threshold value can be selected according to the actual situation.
In the embodiment, based on the prior art, the mobile robot calculates the distance from the mobile robot to the elevator and the wall body through the wall body characteristics of the surrounding environment to confirm the current position, judges the distance from the mobile robot to the elevator after moving to the elevator waiting point, counts the number of laser rays larger than 2m to obtain whether the elevator door is opened or not, needs to fit the straight line where the elevator is located and the straight line where the wall body is located, consumes a large amount of computing resources for fitting operation, and once the elevator or the wall body is shielded, the subsequent steps cannot be carried out, the dependence degree on the environment is high, and the anti-interference capability is weak, the embodiment solves the problem that the robot cannot correctly obtain the position of the mobile robot in the prior art, all objects in a map are in a uniform coordinate system through establishing the map, the mobile robot can quickly position the mobile robot and the elevator, and laser ray scanning data are converted into a point set under the coordinate system, the opening and closing state of the elevator door can be rapidly obtained by judging the position relation between each point in the point set and the elevator door, the dependence degree on the environment is low, the anti-interference capability is strong, the calculation mode is simple, and the robustness is good.
Step 104: and generating a second preset number of line segments according to the acquired first preset number of corner point coordinates, and moving the line segments to enable the line segments to form a plurality of rectangular areas with preset sizes.
In this embodiment, according to the coordinates of the four corner points in the elevator obtained in step 101, a straight line is fictionally constructed in the elevator, and the straight line is parallel to the straight line where the elevator door is located, that is, the straight line formed by connecting the point a and the point C, and the first preset distance from the straight line to the straight line where the point a and the point C are connected is h, and the value of the first preset distance h should be greater than the sum of the diameter 2r of the robot, the thickness of the elevator door and the allowable positioning error K, the straight line fictitious realization step is that a point E, F is respectively selected from the straight lines where the left and right walls in the elevator car are located, that is, AB and CD, so that the lengths of AE and CF are h, and 2r is less than h and L and EF// AC, and E, F point coordinates E (x + d + h, y + W/2, z) and F (x + d + h, y-W/2, z) can be obtained according to A, C point coordinates. In this embodiment, a plurality of points are selected on the straight line EF, so that the straight line EF is divided into a second preset number of line segments, an equal number of symmetrical points are correspondingly selected on the straight line where the elevator door is located, i.e., the straight line AC, two points form a line segment, and four points form a rectangular area.
As an example in this embodiment, the number of the plurality of points selected on the straight line EF is W/h +1, where W is the width of the elevator, h is the distance from the straight line EF to the straight line AC, and the line segments of the second preset number are W/h segments. In the embodiment, the step of selecting the point and marking the line segment is to select the point E as the starting point, select a point G on the straight line EF to make the length of EG be h, then obtain the coordinate G (x + d + h, y + W/2-h, z) of the point G according to the coordinate of the point E, and the subsequently selected point coordinate is (x + d + h, y + W/2-n h, z), wherein n represents the serial number of the selected point, and n belongs to [0, W/h ]]Forming a segment by every two continuous points, moving each intercepted segment down by a distance H, namely finding a corresponding segment on a straight line AC, taking a point A as a starting point, taking the coordinate of a point H corresponding to a point G as (x + d, y + W/2-H, z), taking the subsequent corresponding point coordinate as (x + d, y + W/2-n H, z), respectively storing selected points on a straight line EF and a straight line AC into sets a and b, traversing the sets a and b, and respectively taking out the set a and the set bn(x+d+h，y+W/2－n＊h，z)、an+1(x+d+h，y+W/2－(n+1)＊h，z)、bn(x+d，y+W/2－n＊h，z)、bn+1And (x + d, y-W/2- (n +1) h, z) forming a rectangular area of a predetermined size from the four points, and placing the rectangular area into the set c, wherein the predetermined size is h. In this embodiment, the first rectangular region is AEGH, where the first and second points in the set a are E, G, and the first and second points in the set b are A, H, as shown in fig. 4, the coordinates of each vertex of the rectangular region AEGH in the map coordinate system are a (x + d, y + W/2, z), E (x + d + H, y + W/2, z), G (x + d + H, y + W/2-H, z), H (x + d, y-W/2-H, z).
In this embodiment, based on the prior art, the total area detected by the laser beams is obtained by calculating the area formed by two adjacent laser beams within the preset angle range and performing summation, and the area in the elevator is obtained by subtracting the area of the triangle formed by the mobile robot and the elevator door. The operation of calculating the areas of adjacent rays and summing is very complicated, the space detected by the laser rays in the same range is reduced as the mobile robot moves forward, and whether the elevator can enter the elevator or not can not be accurately judged in real time. According to the embodiment, each rectangular area can be rapidly and conveniently solved through the unified coordinate system, the areas are unchanged relative to the map, and the space in the area can be accurately judged in real time even if the mobile robot moves forwards.
Step 105: and traversing the point set, counting the number of points falling in each rectangular area, comparing the number of points in each rectangular area with a preset second threshold value, and judging whether the robot enters the elevator.
In this embodiment, traverse the point set to obtain the current point coordinates (X, Y, Z), determine whether the current point of the point set satisfies the interval of X + d, X + d + h with the ordinate being in the interval of Y + W/2-n h, Y + W/2- (n +1) h according to the W/h rectangular areas in the set c generated in step 104, if so, determine that the point falls into the (Y-W/2)/h rectangular areas, add 1 to the number of points satisfying the condition in the rectangular areas to finally obtain the number of scattered points in each rectangular area, store the number in the set d, traverse the set d, if the number of points falling into the rectangular areas is less than the preset second threshold, record the number k of the area, and if there are a plurality of areas satisfying the requirement, determine that the robot can enter the area near the middle of the elevator, i.e., the minimum value of k- (n/2); and if the number of the points falling into the rectangular area is larger than or equal to a preset second threshold value, entering the next rectangular area for judgment, and after the set c is traversed, judging that the robot needs to wait for the next elevator if all the rectangular areas do not meet the conditions. The parameter of the preset second threshold value can be set according to actual conditions.
In the embodiment, the point falling conditions of all the rectangular areas can be obtained only by traversing the point set once, so that the time-consuming step of judging the point set by traversing each rectangular area once is omitted, and the mobile robot is ensured to keep the established route ahead under the condition of meeting the entering condition by following the principle of the vicinity.
In this embodiment, if the number of points falling into the first rectangular area GHIJ is greater than or equal to the preset second threshold, the robot enters the second rectangular area for determination, traverses the plurality of rectangular areas with the side length of h generated in step 104, and if all the rectangular areas do not satisfy that the number of points falling into the rectangular areas is less than the preset second threshold, determines that the robot needs to wait for the next elevator.
Referring to fig. 2, fig. 2 is a schematic structural diagram of an embodiment of a real-time detection apparatus for a passable area provided by the present invention, as shown in fig. 2, the apparatus includes an obtaining module 201, a data processing module 202, a first determining module 203, an area dividing module 204, and a second determining module 205, which are as follows:
the obtaining module 201 is configured to obtain coordinates of the elevator waiting points, so that the robot navigates to the elevator waiting points, and calculate a first preset number of corner coordinates formed by the single line laser radar scanning plane and the elevator according to system parameters.
In this embodiment, a map is established to facilitate generation of a unified map coordinate system, the positions of the robot and the elevator are accurately obtained, the robot navigates to a waiting point P (x, y, z) in front of the elevator according to the obtained coordinates of the waiting point of the elevator based on a self-positioning navigation system, and the position of the robot at the waiting point is made to face the elevator door by adjusting the posture. In this embodiment, the robot adopts the right-hand coordinate system, and after the robot navigates to the elevator waiting point, it is assumed that the direction of the right-hand coordinate system of the robot is consistent with the direction of the map coordinate system, and if the direction of the coordinate system in the right hand of the robot is inconsistent with the direction of the map coordinate system, the direction of the robot can be adjusted according to the posture adjustment system.
As a preferable solution in this embodiment, the first preset number is set to 4, and similarly, the first preset number may be adjusted according to actual situations. In this embodiment, as shown in fig. 3, the system parameters include the length L and width W of the elevator, the radius r of the robot, and the distance d from the elevator waiting point to the elevator; according to system parameters, coordinates of four corner points of the elevator on a single-line laser radar scanning surface relative to coordinates under map are respectively A (x + D, y + W/2, z), B (x + D + L, y + W/2, z), C (x + D, y-W/2, z) and D (x + D + L, y-W/2, z).
The data processing module 202 is configured to acquire point cloud data by emitting laser rays through a single-line laser radar, and process the point cloud data to generate a point set.
In the embodiment, the robot transmits a plurality of laser rays through a single line laser radar in a visual angle of a left-right symmetrical deviation of the elevator at a certain angle, obtains a distance value of the plurality of laser rays reflected back from an object, screens the distance value, keeps the distance value obtained by the laser rays transmitted from the visual angle of the robot right facing the left 90 degrees of the elevator door to the visual angle of the robot right facing the right 90 degrees of the elevator door, and obtains original point cloud data corresponding to the distance value kept after screening, wherein the original point cloud data mainly comprises polar coordinates of a plurality of point clouds. In this embodiment, adopt the mode of single line laser radar transmission laser, it is little influenced by the environment, and the testing result is more stable high-efficient, and the single line laser radar price is low, can further reduce the detection cost.
In the embodiment, the acquired point cloud data has the characteristics of high stability and high precision, the point cloud data volume is small, the influence of the environment is low, the algorithm is simple, the processing is convenient, the installation position of the single-line laser radar can be adjusted, the method is used for detecting elevator cars with different heights, and the application range is wide.
In this embodiment, the polar coordinates (ρ, θ) of the point cloud corresponding to the original point cloud data are converted into rectangular coordinates (x, y) according to a coordinate conversion formula, where the coordinate conversion formula is as follows:
in the implementation, the point cloud after coordinate conversion is subjected to voxel down-sampling and Euclidean clustering processing respectively, so that the purposes of removing noise points and reserving effective detection objects are achieved; and simultaneously, obtaining a transformational relation from a radar coordinate system to a map coordinate system by obtaining a tf tree of the robot, and converting the point cloud subjected to voxel downsampling and Euclidean clustering into a point set Q under the map coordinate system according to the transformational relation.
The first judging module 203 is configured to obtain the abscissa corresponding to each point in the point set, compare the number of points whose value is greater than the value of the abscissa of the preset angular point with a preset first threshold, and judge the opening and closing condition of the elevator door.
In this embodiment, the point set is traversed to obtain a numerical value i of the abscissa in the point set. Meanwhile, based on the positioning deviation and the thickness of the elevator door, presetting an allowable error K, comparing a numerical value i of an abscissa of a current point with a numerical value of an abscissa of a preset angular point, wherein the preset angular point is a point A, the numerical value of the abscissa of the point A is x + r + d + K, if the numerical value i of the abscissa of the current point is greater than the numerical value of the abscissa of the point A, the current point is considered as a point falling in the elevator, counting the number of points, in which the numerical value i of the abscissa of the point set is greater than the number of the numerical value of the abscissa of the point A, if the number of the points, in which the numerical value i of the abscissa of the point set is greater than the numerical value of the abscissa of the point A, is less than a preset first threshold value, judging that the elevator door is in an incompletely opened state, and if the number of the points, in which the numerical value i of the abscissa of the point set is greater than the numerical value of the point A, is greater than or equal to the preset first threshold value, judging that the elevator door is opened.
In the prior art, a mobile robot calculates the distance from the mobile robot to an elevator and a wall body through the wall body characteristics of the surrounding environment to confirm the current position, judges that the distance from the mobile robot to the elevator is equal to the distance from the mobile robot to the wall body, then counts the number of laser rays of more than 2m to obtain whether an elevator door is opened or not when the elevator door is moved to a waiting elevator point, needs to fit the straight line where the elevator door is positioned and the straight line where the wall body is positioned, consumes a large amount of calculation resources for fitting operation, and cannot perform the subsequent steps once the elevator or the wall body is shielded, has high dependence degree on the environment and weak anti-interference capability, solves the problem that the robot cannot correctly obtain the position of the mobile robot when shielding exists in the prior art, ensures that all objects in a map are in a uniform coordinate system through establishing the map, can quickly position the mobile robot and the elevator, and converts laser ray scanning data into a point set under the coordinate system, the opening and closing state of the elevator door can be rapidly obtained by judging the position relation between each point in the point set and the elevator door, the dependence degree on the environment is low, the anti-interference capability is strong, the calculation mode is simple, and the robustness is good.
In this embodiment, the abscissa of the point set is directly obtained to determine and reduce the calculation amount of the distance from the point to the point, the calculation speed is increased, and the preset allowable error K and the preset first threshold value can be selected according to the actual situation.
The region dividing module 204 is configured to generate a second preset number of line segments according to the acquired first preset number of corner coordinates, and move the line segments so that the line segments form a plurality of rectangular regions with preset sizes.
In this embodiment, according to the coordinates of four corner points in the elevator obtained in the obtaining module 201, a straight line is virtually constructed in the elevator, so that the straight line is parallel to a straight line where the elevator door is located, that is, a straight line formed by connecting the point a and the point C, a first preset distance from the straight line to the straight line where the point a and the point C are connected is h, and a value of the first preset distance h should be greater than a sum of a diameter 2r of the robot, a thickness of the elevator door and a positioning allowable error K, and the straight line is virtually constructed by selecting a point E, F on the straight line where the left wall and the right wall are located, that is, AB and CD, so that lengths of AE and CF are h, and 2r is less than h and L and EF// AC, and then E, F point coordinates E (x + d + h, y + W/2, z) and F (x + d + h, y-W/2, z) can be obtained according to A, C point coordinates.
In this embodiment, a plurality of points are selected on the straight line EF, so that the straight line EF is divided into a second preset number of line segments, an equal number of symmetrical points are correspondingly selected on the straight line where the elevator door is located, i.e., the straight line AC, two points form a line segment, and four points form a rectangular area. As an example in this embodiment, the number of the plurality of points selected on the straight line EF is W/h +1, where W is the width of the elevator, h is the distance from the straight line EF to the straight line AC, and the line segments of the second preset number are W/h segments. In the embodiment, the step of selecting the point and marking the line segment is to select the point E as the starting point, select a point G on the straight line EF to make the length of EG be h, then obtain the coordinate G (x + d + h, y + W/2-h, z) of the point G according to the coordinate of the point E, and the subsequently selected point coordinate is (x + d + h, y + W/2-n h, z), wherein n represents the serial number of the selected point, and n belongs to [0, W/h ]]Forming a segment by every two continuous points, moving each intercepted segment down by a distance H, namely finding a corresponding segment on a straight line AC, taking a point A as a starting point, taking the coordinate of a point H corresponding to a point G as (x + d, y + W/2-H, z), taking the subsequent corresponding point coordinate as (x + d, y + W/2-n H, z), respectively storing selected points on a straight line EF and a straight line AC into sets a and b, traversing the sets a and b, and respectively taking out the set a and the set bn(x+d+h，y+W/2－n＊h，z)、an+1(x+d+h，y+W/2－(n+1)＊h，z)、bn(x+d，y+W/2－n＊h，z)、bn+1And (x + d, y-W/2- (n +1) h, z) forming a rectangular area of a predetermined size from the four points, and placing the rectangular area into the set c, wherein the predetermined size is h. In this embodiment, the first rectangular region is AEGH, where the first and second points in the set a are E, G, and the first and second points in the set b are A, H, as shown in fig. 4, the coordinates of each vertex of the rectangular region AEGH in the map coordinate system are a (x + d, y + W/2, z), E (x + d + H, y + W/2, z), G (x + d + H, y + W/2-H, z), H (x + d, y-W/2-H, z).
In the prior art, the total area detected by the laser rays is obtained by calculating the area formed by two adjacent laser rays within a preset angle range, accumulating and summing the areas, and the area of a triangle formed by the mobile robot and the elevator door is subtracted to obtain the area in the elevator. The operation of calculating the areas of adjacent rays and summing is very complicated, the space detected by the laser rays in the same range is reduced as the mobile robot moves forward, and whether the elevator can enter the elevator or not can not be accurately judged in real time. According to the invention, through a unified coordinate system, each rectangular area can be rapidly and conveniently solved, the areas are unchanged relative to a map, and the space in the area can be accurately judged in real time even if the mobile robot moves forward.
The second judging module 205 is configured to traverse the point set, count the number of points falling in each rectangular area, compare the number of points in each rectangular area with a preset second threshold, and judge whether the robot enters the elevator.
In this embodiment, traverse the point set to obtain the coordinates (X, Y, Z) of the current point, determine whether the current point of the point set satisfies the interval of X + d, X + d + h with the ordinate being in the interval of Y + W/2-n h, Y + W/2- (n +1) h, according to the W/h rectangular areas in the set c generated by the area dividing module 204, if the condition is satisfied, the point is considered to fall into the (Y-W/2)/h rectangular areas, the number of points satisfying the condition in the rectangular area is added by one to finally obtain the number of points scattered in each rectangular area, store the number into the set d, traverse the set d, if the number of points falling into the rectangular area is smaller than the preset second threshold, record the serial number k of the area, if there are a plurality of areas satisfying the requirement, determine that the robot can enter the area in the middle of the elevator, i.e., the minimum value of k- (n/2); and if the number of the points falling into the rectangular area is larger than or equal to a preset second threshold value, entering the next rectangular area for judgment, and after the set c is traversed, judging that the robot needs to wait for the next elevator if all the rectangular areas do not meet the conditions. The parameter of the preset second threshold value can be set according to actual conditions. In the invention, the point falling condition of all rectangular areas can be obtained only by traversing the point set once, so that the time-consuming step of judging the point set by traversing each rectangular area once is avoided, and the mobile robot is ensured to keep a given route ahead under the condition of meeting the entering condition by following the principle of the vicinity.
In this embodiment, if the number of points falling into the first rectangular region GHIJ is greater than or equal to the preset second threshold, the robot enters the second rectangular region for judgment, traverses a plurality of rectangular regions with a side length of h generated in the region dividing module 204, and if all the rectangular regions do not satisfy that the number of points falling into the rectangular regions is less than the preset second threshold, judges that the robot needs to wait for the next elevator.
In this embodiment, a terminal device is further provided, which includes a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, and when the processor executes the computer program, the real-time detection method of a passable area as described in the above embodiments is implemented.
In this embodiment, a computer-readable storage medium is further provided, where the computer-readable storage medium includes a stored computer program, where when the computer program runs, the apparatus where the computer-readable storage medium is located is controlled to execute the real-time detection method for a passable area according to the foregoing embodiment.
In summary, the invention relates to a real-time detection method, a real-time detection device, a real-time detection terminal and a real-time detection storage medium for passable areas, wherein the method comprises the steps of obtaining coordinates of elevator waiting points to enable a robot to navigate to the elevator waiting points, and calculating a first preset number of angular point coordinates formed by a single-line laser radar scanning plane and an elevator according to system parameters; acquiring point cloud data by emitting laser rays, and processing the point cloud data to generate a point set; acquiring the abscissa corresponding to each point in the point set, comparing the number of points of which the numerical value is greater than the numerical value of the abscissa of the preset angular point with a preset first threshold value, and judging the opening and closing condition of the elevator door; generating a second preset number of line segments according to the acquired first preset number of corner point coordinates, and moving the line segments to enable the line segments to form a plurality of rectangular areas with preset sizes; and traversing the point set, counting the number of points falling in each rectangular area, comparing the number of points in each rectangular area with a preset second threshold value, and judging whether the robot enters the elevator. Compared with the prior art, the method and the device have the advantages that the point cloud data are collected and processed, the elevator car passable area is detected from the two aspects of the state of the elevator door and whether the robot can enter the elevator, the detection efficiency and the accuracy of the detection result are improved, and the passing safety of the robot is ensured.
The above description is only a preferred embodiment of the present invention, and it should be noted that, for those skilled in the art, various modifications and substitutions can be made without departing from the technical principle of the present invention, and these modifications and substitutions should also be regarded as the protection scope of the present invention.
Claims (10)
1. A method for real-time detection of passable areas, comprising:
acquiring coordinates of elevator waiting points to enable the robot to navigate to the elevator waiting points, and calculating a first preset number of angular point coordinates formed by a single line laser radar scanning plane and an elevator through system parameters;
acquiring point cloud data by emitting laser rays through a single-line laser radar, and processing the point cloud data to generate a point set;
acquiring the abscissa corresponding to each point in the point set, comparing the number of points of which the numerical value is greater than the numerical value of the abscissa of a preset angular point with a preset first threshold value, and judging the opening and closing condition of the elevator door;
generating a second preset number of line segments according to the acquired first preset number of corner point coordinates, and moving the line segments to enable the line segments to form a plurality of rectangular areas with preset sizes;
and traversing the point set, counting the number of points falling in each rectangular area, comparing the number of points in each rectangular area with a preset second threshold value, and judging whether the robot enters the elevator.
2. The real-time detection method of the passable area according to claim 1, wherein the point cloud data is collected by emitting laser rays through a single line laser radar, and is processed to generate a point set, specifically:
emitting a plurality of laser rays through a single-line laser radar, obtaining distance values detected by the plurality of laser rays, screening the distance values, and obtaining original point cloud data corresponding to the screened distance values;
performing coordinate conversion on the original point cloud data, and performing voxel down-sampling and Euclidean clustering processing on the original point cloud data after the coordinate conversion;
and simultaneously, generating a point set corresponding to the processed original point cloud data according to the transformation relation of the coordinate system.
3. The method according to claim 1, wherein a second predetermined number of line segments are generated, and the line segments are moved so that the line segments form a plurality of rectangular areas of a predetermined size, specifically:
virtually constructing a straight line in the elevator so that the distance from the straight line to an elevator door is a first preset distance, wherein the first preset distance is greater than the diameter of the robot;
and dividing the straight line into a second preset number of line segments, and moving the line segments back and forth to enable the line segments to form a plurality of rectangular areas with preset sizes.
4. The method of claim 1, wherein the system parameters comprise the length and width of the elevator, the radius of the robot, and the distance from the landing point to the elevator.
5. A real-time detection device for passable areas, comprising: the system comprises an acquisition module, a data processing module, a first judgment module, a region division module and a second judgment module, and specifically comprises:
the acquisition module is used for acquiring coordinates of the elevator waiting points so that the robot can navigate to the elevator waiting points, and calculating a first preset number of angular point coordinates formed by a single line laser radar scanning plane and the elevator according to system parameters;
the data processing module is used for acquiring point cloud data by emitting laser rays through a single-line laser radar, processing the point cloud data and generating a point set;
the first judging module is used for acquiring the abscissa corresponding to each point in the point set, comparing the number of points of which the numerical value is greater than the numerical value of the abscissa of the preset angular point with a preset first threshold value, and judging the opening and closing condition of the elevator door;
the region dividing module is used for generating line segments of a second preset number according to the acquired corner coordinates of the first preset number, and moving the line segments to enable the line segments to form a plurality of rectangular regions of preset sizes;
the second judging module is used for traversing the point set, counting the number of points falling in each rectangular area, comparing the number of points in each rectangular area with a preset second threshold value, and judging whether the robot enters the elevator.
6. The device according to claim 5, wherein the data processing module is configured to collect point cloud data by emitting laser beams through a single line lidar, and process the point cloud data to generate a point set, specifically:
emitting a plurality of laser rays through a single-line laser radar, obtaining distance values detected by the plurality of laser rays, screening the distance values, and obtaining original point cloud data corresponding to the screened distance values;
performing coordinate conversion on the original point cloud data, and performing voxel down-sampling and Euclidean clustering processing on the original point cloud data after the coordinate conversion;
and simultaneously, generating a point set corresponding to the processed original point cloud data according to the transformation relation of the coordinate system.
7. The device for real-time detection of passable areas according to claim 5, wherein the area dividing module is configured to generate a second predetermined number of line segments, and move the line segments so that the line segments form a plurality of rectangular areas with predetermined sizes, specifically:
virtually constructing a straight line in the elevator so that the distance from the straight line to an elevator door is a first preset distance, wherein the first preset distance is greater than the diameter of the robot;
and dividing the straight line into a second preset number of line segments, and moving the line segments back and forth to enable the line segments to form a plurality of rectangular areas with preset sizes.
8. The device of claim 5, wherein the system parameters in the acquisition module comprise the length and width of the elevator, the radius of the robot, and the distance from the landing point to the elevator.
9. A terminal device, characterized in that it comprises a processor, a memory and a computer program stored in the memory and configured to be executed by the processor, the processor implementing the real-time detection method of a passable area according to any one of claims 1 to 4 when executing the computer program.
10. A computer-readable storage medium, comprising a stored computer program, wherein the computer program, when executed, controls an apparatus in which the computer-readable storage medium is located to perform the real-time detection method of a passable area according to any one of claims 1 to 4.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111346188.1A CN114200472A (en) | 2021-11-12 | 2021-11-12 | Real-time detection method and device for passable area, terminal and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111346188.1A CN114200472A (en) | 2021-11-12 | 2021-11-12 | Real-time detection method and device for passable area, terminal and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114200472A true CN114200472A (en) | 2022-03-18 |
Family
ID=80647454
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111346188.1A Pending CN114200472A (en) | 2021-11-12 | 2021-11-12 | Real-time detection method and device for passable area, terminal and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114200472A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115123897A (en) * | 2022-08-30 | 2022-09-30 | 深圳市海清视讯科技有限公司 | Object detection method, device and equipment |
| WO2024027210A1 (en) * | 2022-08-03 | 2024-02-08 | 北京京东乾石科技有限公司 | Control method and apparatus for robot, electronic device, and storage medium |
-
2021
- 2021-11-12 CN CN202111346188.1A patent/CN114200472A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2024027210A1 (en) * | 2022-08-03 | 2024-02-08 | 北京京东乾石科技有限公司 | Control method and apparatus for robot, electronic device, and storage medium |
| CN115123897A (en) * | 2022-08-30 | 2022-09-30 | 深圳市海清视讯科技有限公司 | Object detection method, device and equipment |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111258320B (en) | Robot obstacle avoidance method and device, robot and readable storage medium | |
| CN114200472A (en) | Real-time detection method and device for passable area, terminal and storage medium | |
| CN108958282A (en) | Three-dimensional path planing method based on dynamic spherical window | |
| CN110794406A (en) | Multi-source sensor data fusion system and method | |
| CN111368607A (en) | Robot, obstacle detection method and detection device | |
| WO2021082380A1 (en) | Laser radar-based pallet recognition method and system, and electronic device | |
| CN114061486A (en) | Automatic measuring device and method for large-scale skin curved surface of airplane | |
| Dong et al. | A weld line detection robot based on structure light for automatic NDT | |
| CN113475976B (en) | Method and device for determining passable area of robot, storage medium and robot | |
| Nevado et al. | Obtaining 3D models of indoor environments with a mobile robot by estimating local surface directions | |
| Kawashima et al. | Finding the next-best scanner position for as-built modeling of piping systems | |
| CN112561998A (en) | Robot positioning and autonomous charging method based on three-dimensional point cloud registration | |
| CN114764885A (en) | Obstacle detection method and device, computer-readable storage medium and processor | |
| CN112987748A (en) | Robot narrow space control method and device, terminal and storage medium | |
| Donoso-Aguirre et al. | Mobile robot localization using the Hausdorff distance | |
| CN113768420B (en) | Sweeper and control method and device thereof | |
| Liu et al. | A localizability estimation method for mobile robots based on 3d point cloud feature | |
| Akbarally et al. | 3D robot sensing from sonar and vision | |
| CN112734619B (en) | Free-form surface coverage viewpoint automatic sampling method for detecting feature scanning uncertainty | |
| CN115578457A (en) | Dynamic box body detection method applied to unordered grabbing | |
| CN113791400A (en) | Laser radar-based stair parameter autonomous detection method | |
| Pei et al. | A robot ultrasonic mapping method based on the gray system theory | |
| WO2023109541A1 (en) | Autonomous mobile device and control method and apparatus therefor and storage medium | |
| CN112836681B (en) | Obstacle marking method and device and readable non-transitory storage medium | |
| Tang et al. | An approach of dynamic object removing for indoor mapping based on UGV SLAM |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |