Method for recognizing and reconstructing glass in three-dimensional mapping and navigation of mobile robot

Abstract

The application discloses a method for recognizing and reconstructing glass in three-dimensional mapping and navigation of a mobile robot, which comprises the following steps: (1) reflection intensity feature screening, obtaining a candidate point queue PointQueue and a point cloud set SubCloud around each point in the queue; (2) local structure feature screening, judging whether the local structure feature meets a standard or not, and deleting a point P and the point cloud set SubCloud around the point P which do not meet the standard; and (3) reconstructing a glass point cloud, and obtaining a complete point cloud shape. The application only uses one sensor of a laser radar to reduce complexity, combines reflection intensity features and point cloud local structure features to judge, fully utilizes physical features of mirror reflection and rich geometric information of three-dimensional point clouds different from two-dimensional point clouds, and has high recognition efficiency and high accuracy, and can be applied to a simultaneous localization and mapping algorithm to construct a complete indoor glass environment map.

Description

Technical Field

[0001] This invention relates to environmental perception for mobile robots, and in particular to environmental perception and 3D point cloud reconstruction for glass detection in indoor environments with transparent obstacles such as glass, belonging to the technical field of mobile robots. Background Technology

[0002] Robotics technology has developed rapidly in the past decade. Various types of mobile robots have played a significant role in emergency rescue, home services, and environmental exploration. With the development of various sensors and the application of robot algorithms, the work of mobile robots has gradually involved all aspects of human life, often replacing humans in completing various complex tasks.

[0003] The prerequisite for mobile robots to achieve all these functions is the ability to accurately perceive and process surrounding environmental information. Due to the high accuracy and stability of LiDAR data, it is commonly used as the primary sensor to collect environmental information during mobile robot localization, mapping, and navigation. As an optical sensor, LiDAR relies on emitting and receiving reflected laser beams to acquire environmental information. For objects with low diffuse reflectivity, such as glass, mirrors, metal, or ceramic tiles, the diffuse reflection intensity of the light beam is weak and difficult for LiDAR to receive and identify. LiDAR may ignore these low-diffuse-reflectivity obstacles and obtain incorrect object positions. Taking transparent obstacles like glass as an example, the laser beam can pass through the transparent obstacle to directly obtain information about objects on the other side. Therefore, LiDAR cannot effectively detect transparent obstacles in indoor environments. This often leads to problems such as missing grid or point cloud map content, path planning failures based on this map, and even safety issues like falls and collisions when mobile robots perform tasks in indoor settings such as schools, office buildings, and shopping malls. It is worth noting that modern architecture frequently uses glass to improve lighting and enhance aesthetics. This significant increase in the use of glass further restricts the movement range of mobile robots. These transparent obstacles in buildings mainly manifest as glass curtain walls, glass doors, and glass railings.

[0004] For environmental perception in transparent obstacle environments, other proposed glass recognition methods based on lidar sensors have shortcomings. For example, combining lidar with other sensors such as ultrasonic sensors and polarized light cameras for data fusion is often complex in terms of installation and processing algorithms. Recognition using lidar's multi-echo mode based on glass reflection characteristics requires lidar products with multi-echo functionality, limiting its applicability. Matching all point clouds involving specular reflection based on glass specular reflection characteristics is too resource-intensive for large-scale 3D point clouds. Recognition based solely on point cloud reflection intensity or geometric features is prone to errors in practical use, easily overlooking actual glass or incorrectly identifying non-transparent obstacles. Summary of the Invention

[0005] This invention addresses the shortcomings of environmental perception technology for mobile robots in transparent obstacle environments by providing a highly efficient and accurate method for identifying and reconstructing glass in 3D mapping and navigation of mobile robots. This method corrects erroneous point clouds that detect obstacles behind the robot due to penetration of transparent obstacles, and reconstructs the correct and complete shape of the transparent obstacle, thus providing conditions for safe collision avoidance for the robot.

[0006] The method for glass identification and reconstruction in 3D mapping and navigation of mobile robots of the present invention is a method for identifying glass and reconstructing the point cloud using a 3D lidar point cloud. The processing of the lidar point cloud includes three steps: (1) reflection intensity feature screening; (2) local structure feature screening; and (3) glass point cloud reconstruction. The global coordinate system used in each step is a rectangular coordinate system, with the origin located at the center of the lidar, the lidar orientation as the x-axis, the y-axis at 90° to the left of the lidar orientation, and the z-axis perpendicular to the XOY plane and upward.

[0007] The specific procedures for each step are described below.

[0008] (1) Filter by reflection intensity feature to obtain a candidate point queue PointQueue and a point cloud set SubCloud around each point;

[0009] ① The input is a 3D point cloud obtained by a LiDAR scanner, within a certain range R (typically R is a sphere with the LiDAR scanner as the center and a radius of 1.5m). Filtering is performed when the intensity is greater than the threshold Tr. i The point, Tr i The value is set according to the ambient light conditions, and points with the required reflection intensity are formed into a candidate queue PointQueue. The points in PointQueue are arranged in descending order of reflection intensity, and points with lower reflection intensity values ​​near points with high reflection intensity values ​​are removed from PointQueue.

[0010] In step (1)①, the range represented by the point with a large reflection intensity value is a circle with that point as the center and a radius of r = 0.268 × Norm, where Norm is the length of the line connecting that point to the origin of the global coordinate system.

[0011] ② After obtaining the PointQueue, it will iterate through each point it contains and perform calculations. For each point P and its glass plane G, a glass plane coordinate system and a transformation matrix T from the global coordinate system to the glass plane coordinate system will be established. All points in the vicinity of point P will be collected to form a point cloud SubCloud. The collection range is a cylindrical area with upper and lower cylindrical surfaces parallel to the glass plane G. The geometric center of the cylinder is point P, the radius of the cylinder is R, and the height of the cylinder is H.

[0012] The specific process of establishing the glass plane coordinate system and the transformation matrix T from the global coordinate system to the glass plane coordinate system in (1)② is as follows:

[0013] Let the equation of the glass plane G in the global coordinate system be Ax + By + Cz + D = 0, and let the parameters A, B, and C be the normal vectors of the glass plane. In the coordinate system, x, y, and z are the coordinates, and the parameter D of the plane is the length of the line connecting point P to the origin of the global coordinate system. Since point P, where the glass reflection intensity is at its maximum, is obtained by incident along the normal to the glass plane G, the line connecting point P to the origin of the global coordinate system is the normal vector of the glass plane G. Define the normal vector The direction is from the origin of the global coordinate system to point P, and the coordinate system of the glass plane G. With global coordinate system The relationship is shown in the following formula:

[0014]

[0015]

[0016]

[0017] Let the basis of the above coordinate system be... In the global coordinate system, it is represented as The coordinates of point P in the global coordinate system are represented as P = [p x p y p z ] T Then we obtain the transformation matrix T between the two coordinate systems:

[0018]

[0019] ③ Based on the point cloud SubCloud, discretized intensity features are collected. Centered on point P, a 9×9 grid IntensityMatrix is ​​created along the positive and negative x and y directions of the glass plane coordinate system. The length and width of each cell are set to be proportional to the distance (i.e., the farther the distance, the larger the cell size). Points in the SubCloud are transformed into the glass plane coordinate system. The reflection intensity value of each point is filled into the grid according to its spatial location. When multiple reflection intensities are located in the same cell, the maximum intensity value is retained. Then, the gradient GradientMatrix of IntensityMatrix in the x and y directions is calculated, and the divergence of each cell is calculated to generate DivergenceMatrix. It is then determined whether the divergence value at point P meets the divergence threshold Tr. d If the requirements are not met, then delete the non-compliant point P and its surrounding point cloud set SubCloud, Tr. d The value is set according to the ambient lighting conditions.

[0020] In (1)③, the length of the cell is along the x-axis of the glass plane coordinate system, and the scale is to cover 2 to 3 points in the horizontal direction. The width of the cell is along the y-axis of the glass plane coordinate system, and the scale is to cover 1 to 2 points in the vertical direction.

[0021] This completes the filtering of reflection intensity features, resulting in a candidate point queue PointQueue and a point cloud set SubCloud surrounding each point.

[0022] (2) Local structural feature screening: determine whether the local structural features meet the standard, and delete the point P and the surrounding point cloud set SubCloud that do not meet the standard.

[0023] The local structural features and their judgment criteria in step (2) are as follows:

[0024] ① Sector point count characteristics: Project the points in SubCloud onto the glass plane G, and count the number of points in the sector area at fixed angular intervals with point P as the center; Judgment criteria: Distinguish between the left and right sides with the y-axis direction, and the peak values ​​on both sides should appear in the x-axis direction;

[0025] ②Dispersion: Calculate the variance of the distance from all points on the glass plane G to point P when the SubCloud is projected onto it; Judgment criteria: The relatively fixed structure of the glass point cloud should ensure that the variance is within a certain range.

[0026] ③ Pitch angle feature: For point P, find the two points at the top and bottom of its SubCloud in the y-axis direction of the glass plane G coordinate system, and calculate the angle between the line connecting the origin of the global coordinate system to these two points and the normal of the glass plane G (which can be understood as the pitch angle in the usual sense); this feature can describe the shape of the point cloud. An excessively large pitch angle will be considered as a point cloud not formed by glass; judgment criteria: the pitch angle, elevation angle, and the sum of the pitch angles should each be less than their respective ranges.

[0027] ④ Point cloud size: The point cloud size is the number of points in the SubCloud; Judgment criteria: Too small a size is generally caused by noise.

[0028] The system sequentially determines whether the aforementioned local structural features meet the standards. Each judgment condition or range is set according to the actual situation in practical applications. The glass point cloud is manually marked in the environment. The variance range and pitch angle range are determined by the currently known glass point cloud. Point P that does not meet the standards and its surrounding point cloud set, SubCloud, are deleted.

[0029] (3) Reconstruct the glass point cloud;

[0030] ① For flat glass with a frame, the following method is used to obtain the complete point cloud shape:

[0031] First, determine the left and right boundaries; with elevation angle A... e , depression angle A d Using point P as the starting point, search along the negative x-axis of the glass plane G to find an environment where the elevation angle is greater than A. e Angle of depression greater than A d The spatial continuous point cloud is used as the left boundary. From the left and right boundaries, the upper and lower boundaries are found in the positive and negative y-directions of the glass plane G. This yields two upper boundaries and two lower boundaries; the common portion of these boundaries is taken. The boundaries are represented in spherical coordinates, with the left and right boundaries represented by two θ angles, and the upper and lower boundaries represented by two... Angle representation;

[0032] Calculate whether each laser beam is contained within the area defined by the boundary, calculate the length ρ using the following formula, and then modify the position of the point of impact of the laser beam contained within the area onto the glass plane G.

[0033]

[0034] In the formula, A, B, C, and D are the plane equations in the rectangular coordinate system: Ax + By + Cz + D = 0. The origin of the spherical coordinate system is defined as the same as the origin of the lidar coordinate system. Let P be a point on the plane, ρ be the distance between point P and the origin, and θ be the angle between the projection of the line connecting point P and the origin onto the XOY plane and the positive x-axis. It is the complementary angle between the line connecting the two sides and the positive z-axis.

[0035] ② For curved glass or frameless flat glass, the following method is used to obtain the complete point cloud shape:

[0036] If the bounding boxes cannot be found on both sides of the glass point cloud, the glass point cloud is reconstructed using historical pose information. This part of the algorithm requires obtaining the pose transformation matrix T' of the mobile robot in the i-th frame and the (i-1)-th frame of the point cloud. i The distance was obtained from an open-source lidar odometry calculation method.

[0037] In each frame of data obtained by the LiDAR, the transformation matrix T (see step (1) ②) for which the point cloud of the bounding glass could not be found is retained, denoted as T. (i,j) , i is the global coordinate system of the i-th frame where the current glass point cloud is located, j is the order of the j-th frameless glass point cloud in the current frame, with the initial robot pose as the world coordinate system;

[0038] By using various transformation matrices, the pose relationship between any two glass planes is obtained. Starting from the first frame of laser point cloud, the following operations are performed on each newly obtained borderless glass point cloud:

[0039] (a) Distinguish between left and right based on the normal of the current frame's glass plane G. The left side is the negative x-axis of the glass plane coordinate system, and the right side is the opposite. Use a quadtree method to find the two points P on the left and right sides that are closest to the current glass plane. L and P R (Using the quadtree method), P L and P R It is the point with the highest intensity in the bounding box point cloud that was not found in the historical frame (i.e., point P in its respective point cloud), and its distance D is obtained. L and D R ;

[0040] (b) Determine D L and D R Is it less than the distance threshold Tr? p Tr p Based on the scale of the current environment, remove P and D that do not meet the requirements;

[0041] (c) Perform point cloud reconstruction on the side that meets the above requirements. Taking the reconstruction of the left side of the current glass point cloud as an example, point P and point P L Let P be the two ends of the reconstructed point cloud shape, and let the two planes intersect at point P in the viewpoint facing the negative z-axis of the world coordinate system. M Then P, P L P M The second-order Bézier curve drawn for the vertex is used as the shape of the glass point cloud on this side, and the formula for this part of the shape is as follows:

[0042] P(t)=(1-t) 2 P+2t(1-t)P M +t 2 P L ,t∈[0,1];

[0043] (d) Store each completed curve shape in a tree structure, with the tree nodes being the curve endpoints and the branches represented by the second-order Bézier curve equation.

[0044] This invention uses lidar to identify and detect transparent obstacles indoors. By extracting, filtering, and analyzing the lidar point cloud based on reflection intensity characteristics and local point cloud structure features, the pose of the transparent obstacle relative to the robot is calculated. A point cloud reconstruction method is proposed to correct erroneous point clouds that detect obstacles behind the robot due to penetration of the transparent obstacle, and to reconstruct the correct and complete shape of the transparent obstacle, thus providing conditions for the robot to safely avoid collisions.

[0045] The method proposed in this invention reduces complexity by using only a single LiDAR sensor. The algorithm combines reflection intensity characteristics with local point cloud structure features for judgment, fully utilizing the physical characteristics of specular reflection and the rich geometric information of 3D point clouds compared to 2D point clouds. This results in high recognition efficiency and accuracy, and can be applied to Simultaneous Localization and Mapping (SLAM) algorithms to construct complete maps of indoor glass environments. This invention can be applied to point clouds generated by various types of LiDAR, making it widely applicable. Attached Figure Description

[0046] Figure 1 It is a graph showing the relationship between the reflection intensity of different materials and the angle of incidence.

[0047] Figure 2 This is a visual diagram of the glass reflectivity in three-dimensional space.

[0048] Figure 3 This is a flowchart of the reflection intensity screening procedure in this invention.

[0049] Figure 4 This is a schematic diagram of the global coordinate system and the glass plane coordinate system.

[0050] Figure 5 This is a schematic diagram of the area covered by the point cloud data collection.

[0051] Figure 6 This is a schematic diagram showing the characteristics of the number of sector points.

[0052] Figure 7 These are schematic diagrams of the glass railing and its original point cloud; (a) is the glass railing, and (b) is the original point cloud.

[0053] Figure 8 This is a schematic diagram of the point cloud after reconstruction.

[0054] Figure 9 This is a schematic diagram of the transformation matrix.

[0055] Figure 10 This is a schematic diagram of the shape of glass.

[0056] Figure 11 This is a flowchart for reconstructing curved or frameless glass.

[0057] Figure 12 A schematic diagram of the experimental environment and point cloud of the frameless curved glass; (a) is the experimental environment of the frameless curved glass, (b) is the original point cloud from the top view, and (c) is the original point cloud from the front view.

[0058] Figure 13 A schematic diagram of the reconstruction results of the frameless curved glass; (a) is a top view of the reconstructed point cloud of the frameless curved glass, and (b) is a front view of the reconstructed point cloud of the frameless curved glass. Detailed Implementation

[0059] The method for glass identification and reconstruction in 3D mapping and navigation of mobile robots of the present invention is a method for identifying glass and reconstructing the point cloud using a 3D lidar point cloud. The processing of the lidar point cloud includes three steps: (1) reflection intensity feature screening; (2) local structure feature screening; and (3) glass point cloud reconstruction. The global coordinate system used in each step is a rectangular coordinate system, with the origin located at the center of the lidar, the lidar orientation as the x-axis, the y-axis at 90° to the left of the lidar orientation, and the z-axis perpendicular to the XOY plane and upward.

[0060] The specific procedures for each step are described below.

[0061] Step 1: Screening based on reflection intensity characteristics

[0062] When the light beam is incident along the glass normal (i.e., at 0°), it receives extremely high reflection intensity. Within a very small range near 0°, the reflection intensity decreases rapidly. In contrast, the reflection curves of other materials such as walls and metal doors show a gentler decrease and have lower peak reflection intensity. The horizontal reflection intensity received by various materials at different incident angles is as follows: Figure 1 As shown, the range of reflection intensity is 0-255. Furthermore, due to the isotropic nature of the material, both theoretically and through actual observation, the relationship between reflection intensity and incident angle in the vertical direction exhibits the same variation characteristics as in the horizontal direction. Because the horizontal measurement interval of the lidar is 0.2° while the vertical measurement interval is 2°, the acquisition scales in the horizontal and vertical directions are different, resulting in the actual reflection intensity distribution of the obtained point cloud data as shown below. Figure 2 As shown.

[0063] The process flow for filtering reflection intensity features is as follows: Figure 3 The input is a 3D point cloud obtained by a LiDAR scanner. Since glassy point clouds only appear within a certain distance in reality, affecting tasks such as SLAM and navigation, this method operates within a certain range R, typically a sphere with the LiDAR scanner as its center and a radius of 1.5m. The filtering intensity is greater than a threshold Tr. i The point, Tr i The value is set based on the ambient light conditions, and points with the required reflection intensity are formed into a candidate queue, PointQueue. The points in PointQueue are arranged in descending order of reflection intensity. Generally, the reflection intensity of glass is generally greater than that of other objects in the environment. Therefore, points with lower reflection intensity values ​​near points with high reflection intensity values ​​will be deleted from PointQueue. The range "near points with high reflection intensity values" refers to a circle with that point as the center and a radius of r = 0.268 × Norm, where Norm is the length of the line connecting that point to the origin of the global coordinate system.

[0064] After obtaining the PointQueue, each point within it will be traversed and calculated. For each point P and its corresponding glass plane G, a glass plane coordinate system and a transformation matrix T from the global coordinate system to the glass plane coordinate system will be established. Let the equation of the glass plane G in the global coordinate system be Ax + by + Cz + D = 0, and let the parameters A, B, and C be the glass plane normal vectors. In this context, x, y, and z are the coordinates, and the parameter D of the plane is the length of the line connecting point P to the origin of the global coordinate system. Since point P, where the glass reflection intensity is at its maximum, is obtained by incident along the normal to the glass plane G, the line connecting point P to the origin of the global coordinate system can be considered as the normal vector of the glass plane G. Define the normal vector The direction is from the origin of the global coordinate system to point P, and the coordinate system of the glass plane G. With global coordinate system The relationships are shown in equations (1)-(3). Figure 4 This is a schematic diagram of the coordinate system relationship. The plane represented by the glass plane parameters obtained by the above method conforms to the plane where the actual glass is located in reality.

[0065]

[0066]

[0067]

[0068] Let the basis of the above coordinate system be... In the global coordinate system, it is represented as The coordinates of point P in the global coordinate system are represented as P = [px p y p z ] T Then we obtain the transformation matrix between the two coordinate systems:

[0069]

[0070] All points in the vicinity of sampling point P form a point cloud (SubCloud). The sampling range is a cylindrical area with its upper and lower cylindrical surfaces parallel to the glass plane G. Figure 5 As shown. The geometric center of the cylinder is point P, the radius of the cylinder is R, and the height of the cylinder is H, all of which are values ​​determined through experimental debugging.

[0071] Discretized intensity features are collected based on the point cloud SubCloud. A 9×9 grid IntensityMatrix is ​​created centered at point P along the positive and negative x and y directions of the glass plane coordinate system. Considering that point cloud density is inversely proportional to the distance from the origin, the length and width of each cell are set to be proportional to the distance; that is, the farther the distance, the larger the cell area. The length of the cell is along the x-axis of the glass plane coordinate system, covering 2 to 3 points horizontally, and the width is along the y-axis, covering 1 to 2 points vertically. Points in the SubCloud are transformed into the glass plane coordinate system, and the reflection intensity value of each point is filled into the grid according to its spatial location. When multiple reflection intensities are located in the same cell, the maximum intensity value is retained. Then, the gradient GradientMatrix of IntensityMatrix in the x and y directions is calculated, and the divergence of each cell is calculated to generate DivergenceMatrix. Finally, it is determined whether the divergence value at point P meets the divergence threshold Tr. d If the requirements are not met, then delete the non-compliant point P and its surrounding point cloud set SubCloud, Tr. d The value is set according to the ambient lighting conditions.

[0072] This completes the filtering of reflection intensity features, resulting in a candidate point queue PointQueue and a point cloud set SubCloud surrounding each point.

[0073] Step 2: Local Structural Features

[0074] After the above-mentioned filtering based on reflection intensity, some non-transparent obstacles that meet the reflection intensity requirements will still be retained. Considering the rich geometric features contained in the 3D point cloud, after the reflection intensity filtering, some local structural features are extracted from the local point cloud SubCloud of each point P for further filtering. The local structural features and their principles are as follows.

[0075] 1. Sector point quantity characteristics:

[0076] The overall shape of the glass point cloud observed on the glass plane is a circle centered at point P or an ellipse with its major axis along the x-axis. Furthermore, due to the dense sampling points in the horizontal direction and sparse sampling points in the vertical direction of the lidar, the points in the SubCloud are projected onto plane G. The number of points within a sector region is then counted at fixed angular intervals centered at point P. Figure 6 As shown. Judgment criteria: Distinguish between the left and right sides along the y-axis; the peak values ​​on both sides should appear along the x-axis.

[0077] 2. Degree of dispersion:

[0078] Calculate the variance of the distances from all points on plane G projected from SubCloud to point P. Criterion: The relatively fixed structure of the glass point cloud should ensure that the variance is within a certain range.

[0079] 3. Pitch angle characteristics:

[0080] For point P, find the two points at the top and bottom of its SubCloud along the y-axis of the glass plane G coordinate system. Calculate the angle between the line connecting the origin of the global coordinate system to these two points and the normal to the glass plane G; this can be understood as the pitch angle in the usual sense. This feature can describe the shape of the point cloud; excessively large pitch angles will be considered as point clouds not formed by glass. Judgment criteria: the pitch angle, elevation angle, and the sum of the pitch angles should all be less than their respective ranges.

[0081] 4. Point cloud size:

[0082] The point cloud size is the number of points in the subcloud. Criterion: an excessively small size is generally caused by noise.

[0083] The system sequentially determines whether the aforementioned local structural features meet the standards. The judgment conditions or ranges are set according to the actual situation in practical applications. Point P that does not meet the standards and its surrounding point cloud set SubCloud are deleted.

[0084] Step 3: Reconstruct the glass point cloud

[0085] After filtering by reflection intensity features and local structure features, the obtained local point cloud is the point cloud generated by glass reflection. Its plane G is determined by equations (1)-(3). Next, the complete point cloud of the glass is reconstructed.

[0086] 1. Flat glass with a frame

[0087] For most indoor glass surfaces, which have fixed frames, the following method can be used to obtain a complete point cloud shape.

[0088] First, determine the left and right boundaries. Taking the search for the left boundary as an example, with an elevation angle A... e, depression angle A d Using point P as the starting point, search along the negative x-axis of plane G to find an environment where the elevation angle is greater than A. e Angle of depression greater than A d The spatial continuous point cloud is used as the left boundary. From the left and right boundaries, the upper and lower boundaries are found in the positive and negative y-directions of plane G, respectively. This yields two upper boundaries and two lower boundaries; the common portion of these boundaries is taken. The boundaries are represented in spherical coordinates, with the left and right boundaries represented by two θ angles, and the upper and lower boundaries represented by two... Angle representation, θ and The explanation will be provided below.

[0089] Calculate whether each laser beam is included within the area defined by the boundary. The transformation of the plane equation between the rectangular coordinate system and the spherical coordinate system is as shown in equation (4). Calculate the length ρ. Modify the position of the point hit by the laser beam that is included within the area to the plane G.

[0090]

[0091] In the formula, A, B, C, and D are the plane equations in the rectangular coordinate system: Ax + By + Cz + D = 0. The origin of the spherical coordinate system is defined as the same as the origin of the lidar coordinate system. Let P be a point on the plane, ρ be the distance between point P and the origin, and θ be the angle between the projection of the line connecting point P and the origin onto the XOY plane and the positive x-axis. It is the complementary angle between the line connecting the two sides and the positive z-axis.

[0092] 2. Curved glass or frameless flat glass

[0093] If the bounding boxes cannot be found on both sides of the glass point cloud, the glass point cloud is reconstructed using historical pose information. This part of the algorithm requires obtaining the pose transformation matrix T' of the mobile robot in the i-th frame and the (i-1)-th frame of the point cloud. i This can be obtained using open-source lidar odometry calculation methods.

[0094] In each frame of data obtained by the LiDAR, the transformation matrix T (from step one) for which the bounding glass point cloud could not be found is retained, denoted as T. (i,j) Let ... Figure 9 As shown, the viewpoint in the figure is oriented towards the negative z-axis of the world coordinate system (top view).

[0095] Through various transformation matrices, the pose relationship between any two glass planes was obtained. Starting from the first frame of laser point cloud, the following operations were performed on each newly obtained borderless glass point cloud:

[0096] (1) The left and right sides are distinguished by the normal of the glass plane G in the current frame. The left side is the negative x-axis of the glass plane coordinate system, and the right side is the opposite. The quadtree method is used to find the two points P on the left and right sides that are closest to the current glass plane. L P R P L P R The point P in the historical frame that failed to find the bounding box in the point cloud, where the intensity was the highest, is obtained by measuring its distance D. L D R .

[0097] (2) Determine D L D R Is it less than the distance threshold Tr? p Tr p Based on the scale of the current environment, P and D that do not meet the requirements will be deleted.

[0098] (3) Perform point cloud reconstruction on the side that meets the above requirements. Taking the reconstruction of the left side of the current glass point cloud as an example, point P and point P L Let P be the two ends of the reconstructed point cloud shape, and let the two planes intersect at point P in the viewpoint facing the negative z-axis of the world coordinate system. M Then P, P L P M The second-order Bézier curve drawn for the vertex is used as the shape of the glass point cloud on that side, such as... Figure 10 As shown. The formula for the shape of this part is as shown in equation (6).

[0099] P(t)=(1-t) 2 P+2t(1-t)P M +t 2 P L ,t∈[0,1] (6)

[0100] (4) Store each completed curve shape in a tree structure. The nodes of the tree are the endpoints of the curves, and the branches are represented by the second-order Bézier curve equation.

[0101] The flowchart for this section is as follows: Figure 11 As shown.

[0102] The following are examples of the above methods.

[0103] The LiDAR used is the Robosense RS-LiDAR-16, a 16-line LiDAR with a horizontal measurement angle of 360° and a vertical measurement angle of -15° to 15°. It outputs 320,000 points per second with a measurement accuracy of ±2 cm. The LiDAR data is processed using the ADLINK ROScube PicoTGL, equipped with an Intel Core i5-1145G7E processor and an Ubuntu 20.04 Linux system running on ROS. This ROS environment is responsible for subscribing to and processing the LiDAR data.

[0104] The lidar was placed near a glass railing, a common transparent obstacle in indoor environments. The lidar point cloud data of the glass railing and its components are as follows: Figure 7 As shown, running the program in the method proposed in this invention yields the modified point cloud as follows. Figure 8 As shown, the left and right sides are points formed by the mirror reflection of the metal railing frame and their surrounding point clouds, while the middle point cloud is the points formed by the mirror reflection of the glass and their surrounding point clouds. After filtering based on reflection intensity and local structural features, the metal railing frames on both sides are marked with a lighter color, and the glass in the middle is marked with a darker color. The square markers on both sides of the figure are the border positions found by the program. The point cloud is modified and output within the range of the square border markers.

[0105] Taking a curved transparent obstacle as an example, the experimental environment and its original point cloud are as follows: Figure 12 As shown, (a), (b), and (c) represent the actual environment, the original point cloud from a top-down view, and the original point cloud from a frontal view, respectively. An acrylic sheet is used as the transparent obstacle to be reconstructed. The lidar moves forward at a constant speed in a straight line to obtain a fixed inter-frame transformation matrix T'. i The reconstruction results are as follows Figure 13 As shown, Figure 13 (a) is a top view of the reconstructed point cloud from a frameless curved glass frame. Figure 13 (b) is the front view of the reconstructed point cloud of the frameless curved glass.

Claims

1. A method for recognizing and reconstructing glass in mobile robot three-dimensional mapping and navigation, characterized in that, Comprising the following steps, The global coordinate system in each step is a rectangular coordinate system, the origin of the coordinate system is located at the center of the laser radar, the direction of the laser radar is the x-axis, the direction of the laser radar 90° to the left is the y-axis, and the z-axis is perpendicular to the XOY plane upward; The specific process of each step is as follows: (1) Reflection intensity feature screening, obtaining candidate point queue PointQueue and point cloud set SubCloud around each point in it; ①Input is the three-dimensional point cloud obtained by laser radar, within a certain range , screening points with intensity greater than threshold , The value is set according to the ambient light condition, and the points with required reflection intensity form the candidate queue PointQueue; arrange the points in PointQueue according to the reflection intensity from large to small, and the points with small reflection intensity near the points with large reflection intensity will be deleted from PointQueue; ②After obtaining PointQueue, each point contained in it is traversed and operated, for each point P and the glass plane G it is located in, a glass plane coordinate system is established and a conversion matrix T from the global coordinate system to the glass plane coordinate system; Collect all points within a certain range around point P to form a point cloud SubCloud, the collection range is a cylindrical range parallel to the glass plane G, the geometric center of the cylinder is point P, the cylindrical radius is R, and the cylinder height is H; ③On the basis of the point cloud SubCloud, the discrete intensity features are collected, taking point P as the center, to its glass plane coordinate system 、 positive and negative directions to establish the size of the grid IntensityMatrix, set the cell length, width and distance proportional, SubCloud points into the glass plane coordinate system, according to the spatial location of each point's reflection intensity value filled in the grid, multiple reflection intensity located in a cell when the maximum intensity value; then calculate the IntensityMatrix in 、 direction of the gradient GradientMatrix and calculate the divergence of each cell, generating DivergenceMatrix, to determine whether the divergence value at point P meets the requirements of the divergence threshold , if not, delete the point P and the surrounding point cloud set SubCloud, the value according to the ambient light conditions are set; (2) Local structure feature screening, judging whether the local structure feature meets the standard, deleting the point P and the point cloud set SubCloud around it that do not meet the standard; (3) Reconstructing the glass point cloud to obtain the complete point cloud shape.

2. The method for glass identification and reconstruction in 3D mapping and navigation of mobile robots according to claim 1, characterized in that, The range indicated around the point with a large reflection intensity value in step (1) ① is a circle with the point as the center and a radius of , and Norm is the length of the line connecting the point with the origin of the global coordinate system.

3. The method of claim 1, wherein the glass recognition and reconstruction is performed by the mobile robot using a three-dimensional (3D) mapping and navigation system. The specific process of establishing the glass plane coordinate system and the conversion matrix T from the global coordinate system to the glass plane coordinate system in step (1) ② is as follows: The equation of the glass plane G in the global coordinate system is The parameters A, B, and C of the plane are respectively the components of the glass plane normal vector The parameter D of the plane is the length of the line connecting the point P and the origin of the global coordinate system; since the point P of the maximum glass reflection intensity is obtained by the normal incidence of the glass plane G, the line connecting the point P and the origin of the global coordinate system is the normal vector of the glass plane G, and the direction of the normal vector is set to point from the origin of the global coordinate system to the point P, and the coordinate system of the glass plane G is related to the global coordinate system as shown in the following formula:​​​​​​​ ； ； ； Let the base vectors of the above coordinate system be 、 、 In the global coordinate system, it is expressed as 、 、 The coordinates of point P in the global coordinate system are expressed as The conversion matrix T between the two coordinate systems is obtained:

4. The method for glass identification and reconstruction in 3D mapping and navigation of mobile robots according to claim 1, characterized in that, The long side of the cell in step (1) ③ in the glass plane coordinate system The width of the cell in the horizontal direction, the scale covers 2 to 3 points The width of the cell in the vertical direction, the scale covers 1 to 2 points 5. The method for glass identification and reconstruction in 3D mapping and navigation of mobile robots according to claim 1, characterized in that, The local structure feature and its judgment standard in step (2) are as follows: ①Sector point number feature: Project the points in SubCloud onto the glass plane G, and count the number of points in the statistical sector area at fixed angle intervals with point P as the center; The judgment standard is that the peak values on both sides should appear in the x-axis direction, with the y-axis direction distinguishing the left and right sides; ②Dispersity: Calculate the variance of the distances from all points in SubCloud projected onto the glass plane G to point P; The judgment standard is that the variance should be within a certain range due to the fixed structure of the glass point cloud; ③Pitch angle feature: For point P, find the two points at the uppermost and lowermost positions of its SubCloud in the y-axis direction of its glass plane G coordinate system, and calculate the included angle between the line connecting the global coordinate system origin to the two points and the normal of the glass plane G; This feature can describe the shape of the point cloud, and excessive pitch angle will be considered as a point cloud not formed by glass; The judgment standard is that the sum of the pitch angle, the elevation angle and the pitch angle should be less than the range of each value respectively; ④Point cloud size: The size of the point cloud is the number of points in SubCloud; The judgment standard is that too small size is caused by noise; Determine the above variance range and pitch angle range from the currently known glass point cloud, and sequentially judge whether the above local structure features meet the standard, manually mark the glass point cloud in the environment.

6. The method for glass identification and reconstruction in 3D mapping and navigation of mobile robots according to claim 1, characterized in that, In step (3), the following method is used to obtain the complete point cloud shape for the flat glass with a frame: First, determine the left and right boundaries; with the elevation angle , the depression angle as the upper and lower standards, take point P as the starting point, find the continuous point cloud in the environment that satisfies the elevation angle greater than , the depression angle greater than along the negative direction of the x-axis of the glass plane G as the left boundary; find the upper and lower boundaries from the left and right boundaries respectively to the positive and negative directions of the y-axis of the glass plane G, at this time two upper boundaries and two lower boundaries will be obtained, take the common part of the boundaries; the representation form of the boundary is the range of the spherical coordinate system, the range of the left and right boundaries is represented by two angles, and the orientation of the upper and lower boundaries is represented by two angles; calculating whether each beam of laser light is contained within the range determined by the boundary, calculating the length as modifying the position of the point of impingement of the laser beam contained within the range onto the glass plane G, ； In the formula , , C, is a plane equation in a rectangular coordinate system , the origin of the spherical coordinate system is the same as the origin of the laser radar coordinate system, and P is a point on the plane, is the distance between point P and the origin, is the angle between the projection of the line connecting point P and the origin on the XOY plane and the positive direction of the x-axis, is the complementary angle of the angle between the line and the positive direction of the z-axis.

7. The method for glass identification and reconstruction in 3D mapping and navigation of mobile robots according to claim 1, characterized in that, In step (3), the following method is used to obtain the complete point cloud shape for the curved glass or flat glass without a frame: If the frame is not found on both sides of the glass point cloud, the historical pose information is used to reconstruct the glass point cloud, and the algorithm premise is to obtain the pose transformation matrix of the mobile robot at the i-th frame and the i-1-th frame of the point cloud obtained by the laser radar mileage calculation method In the data obtained by each frame of laser radar, the transformation matrix T which fails to find the edgeless glass point cloud is represented as , i is the i-th frame global coordinate system where the current glass point cloud is located, j is the order of the j-th edgeless glass point cloud under the current frame, and the initial robot pose is the world coordinate system; Through each conversion matrix, the pose relationship between any two glass planes is obtained, and starting from the first frame of laser point cloud, each newly obtained glass point cloud without a frame is operated as follows: (a) with the normal of the current frame glass plane G distinguishing left and right, the left side is the negative direction of the x-axis of the glass plane coordinate system, and the right side is the opposite, the four-ary tree method is used to find the two points closest to the point P on the current glass plane on the left and right sides respectively and , and are the maximum intensity points in the recorded historical frame that fail to find the edge frame point cloud, and the distances and of the two points are obtained (b) determining and whether the distance is less than a distance threshold , P and D that do not meet the requirements are deleted according to the scale of the current environment; (c) Perform point cloud reconstruction on the side that meets the above requirements. When selecting the left side of the current glass point cloud for point cloud reconstruction, point P and point... Let these be the two ends of the reconstructed point cloud shape, and let the two planes intersect at point [point] in the viewpoint oriented towards the negative z-axis of the world coordinate system. Then P, , The second-order Bézier curve drawn for the vertex is used as the shape of the glass point cloud on this side. The formula for this part of the shape is as follows: ； (d) storing each of the completed curve shapes in a tree structure, the nodes of the tree being the end points of the curves and the branches of the tree being represented by second order Bezier curve equations. (d) storing each of the completed curve shapes in a tree structure, the nodes of the tree being the end points of the curves and the branches of the tree being represented by second order Bezier curve equations.

Images