Humanoid robot patrol task interest point searching method

Abstract

The invention discloses a humanoid robot patrol task interest point searching method. The method comprises the following steps: acquiring an environment image by using a camera; detecting a color abnormal area in the environment image, marking the color abnormal area as a point of interest, and carrying out average operation on the pixel area of the environment image to obtain the pixel center coordinate of the point of interest on the environment image; performing mathematical operation by using an internal reference matrix of the camera, converting a pixel center coordinate of the point of interest in a two-dimensional image coordinate system into a coordinate in a camera coordinate system, and performing corresponding matrix transformation and translation transformation on the obtained camera coordinate according to internal and external parameters of the camera to obtain a coordinate of the point of interest; according to the method, after the patroller arrives at the target area, the whole interest target library is divided into a plurality of parts according to the time sequence, so that it is guaranteed that instantaneous errors possibly generated in the operation process of the patroller cannot affect the interest targets found before the errors are generated.

Description

Technical Field

[0001] The invention belongs to the field of humanoid robot patrol navigation, and in particular relates to a method for searching interest points of a humanoid robot patrol mission. Background Art

[0002] Most of the earlier target discovery technologies were based on 2D image target recognition, relying on 2D image data obtained from cameras or other sensors to identify targets of interest. These methods include techniques based on template matching, feature point extraction and matching, and deep learning. However, in lunar exploration missions, 2D images may be affected by factors such as illumination, occlusion, and perspective, resulting in reduced target recognition performance. To overcome the limitations of 2D image-based methods, researchers began to focus on target recognition technologies based on 3D data. These methods use 3D point cloud data obtained by sensors such as lidar and RGBD cameras to identify targets of interest by comparing the geometric structure and features of the point cloud. Common algorithms include techniques based on point pair features (PPF), iterative closest point (ICP), random sample consensus (RANSAC), and deep learning. These methods have better robustness in dealing with complex environments and occlusions.

[0003] With the rapid development of deep learning technology, deep learning-based methods for identifying targets of interest have also achieved remarkable results in the field of lunar exploration. These methods usually use convolutional neural networks (CNNs) or other neural network structures to analyze 2D images or 3D point cloud data to identify targets of interest. Compared with traditional methods, deep learning methods can automatically learn to extract useful features from raw data and have stronger generalization capabilities. However, deep learning methods usually require a large amount of labeled data for training and are relatively expensive in terms of computing and memory requirements, which will be severely resource-constrained in lunar exploration missions.

[0004] The present invention combines the actual engineering constraints on the lunar surface, computing resource limitations and other factors, adopts traditional geometric methods, and uses RGB-D cameras to achieve target matching in a 3D environment, thereby realizing an algorithm for searching targets of interest based on depth information and positioning information. The target discovery of the present invention is based on a clustering algorithm, so the accuracy required for each target search can be appropriately relaxed, and finally the upper and lower color thresholds are used to establish a mask for target discovery, which greatly reduces the time complexity and space complexity of the algorithm.

[0005] In the process of searching for points of interest in images, since the imaging effect of the camera is affected by factors such as light conditions, sensor quality, and surface environment, the points of interest may appear incomplete or blurred in the acquired images, requiring further processing and analysis to determine their exact position and range. In addition, the camera may also capture two targets of interest at the same time. Therefore, it is an urgent problem to propose a method for searching points of interest in lunar patrol detection to ensure the robustness of the system for identifying points of interest. Summary of the invention

[0006] The purpose of the present invention is to provide a method for searching points of interest for a humanoid robot patrol mission, which can ensure that instantaneous errors that may occur during the operation of the patrol device will not affect the targets of interest discovered before the error occurs; and while appropriately relaxing the accuracy required for searching the targets of interest, it also greatly reduces the time complexity and space complexity of the algorithm.

[0007] In order to solve the above technical problems, the technical solution adopted by the present invention is: a method for searching points of interest for a humanoid robot patrol mission, comprising the following steps:

[0008] Step S1: Use a camera to obtain an environment image;

[0009] Step S2: Detecting color abnormal areas in the environment image, marking the color abnormal areas as interest points, and performing an average operation on the pixel areas of the environment image to obtain the pixel center coordinates of the interest points on the environment image;

[0010] Step S3: using the camera's intrinsic parameter matrix to perform mathematical operations, the pixel center coordinates of the interest point in the two-dimensional image coordinate system are converted into coordinates in the camera coordinate system, and according to the camera's intrinsic and extrinsic parameters, the obtained camera coordinates are subjected to corresponding matrix transformation and translation transformation, thereby obtaining the coordinates of the interest point in the patrol coordinate system;

[0011] Step S4: Obtain the coordinates of the interest point in the world coordinate system through the coordinates of the interest point in the patrol coordinate system, and store the coordinates of all the interest points found in the world coordinate system in the order of discovery in the interest target library. After the patroller reaches the target area and finishes patrolling, divide the entire interest target library into m interest targets in chronological order.

[0012] Step S5: cluster the m objects of interest respectively using the DBSCAN algorithm, and select the center point coordinates of the n clusters that meet the conditions as the coordinates of the points of interest.

[0013] Further, the world coordinate system in step S4 is a northeast celestial coordinate system, which is a three-axis mutually orthogonal coordinate system with the starting point as the origin, the X-axis pointing to the north, the Y-axis pointing to the east, and the Z-axis pointing to the sky; wherein the rover coordinate system refers to a three-axis mutually orthogonal coordinate system with the rover's center of mass as the origin, the X-axis pointing to the rover's forward direction, the Y-axis pointing to the left of the rover's forward direction, and the Z-axis pointing to the upward direction; wherein the camera coordinate system refers to a three-axis mutually orthogonal coordinate system with the camera aperture as the origin, the X-axis pointing to the left of the camera's shooting direction, the Y-axis pointing to the downward direction, and the Z-axis pointing to the forward direction; wherein the two-dimensional image coordinate system refers to a two-axis mutually orthogonal coordinate system with the upper left corner of the pixel image as the origin, the Y-axis pointing downward, and the X-axis pointing to the right.

[0014] Furthermore, the interest point in step S2 exists in a certain pixel area of ​​the environment image, and the pixel center coordinates of the interest point are the centroid pixel coordinates of the area.

[0015] Furthermore, the patroller in step S3 is equipped with a front RGB-D camera and a rear RGB-D camera, and there is a certain deflection angle between the axis of the camera lens and the horizontal direction of the patroller, and the deflection angle ranges from 0 to 180 degrees.

[0016] Furthermore, in step S3, the two-dimensional pixel image coordinate system can be mapped to the camera coordinate system to obtain the coordinates of each pixel point relative to the camera coordinate system; and since the position from the camera to the patroller is fixed, the installation matrix is ​​obtained according to the installation position of the camera on the patroller, and the coordinates of the point of interest in the patroller coordinate system can be obtained.

[0017] Furthermore, the initial coordinates and posture of the patroller are known in the world coordinate system, and the displacement of each axis of the patroller in the world coordinate system is known through the patroller's accumulated movement value, and then the coordinates of the interest point relative to the patroller coordinate system are converted into the coordinates of the interest point relative to the world coordinate system through inverse operation.

[0018] Furthermore, in step S5, the method for determining whether the cluster meets the conditions comprises the following steps:

[0019] Step S5.1: cluster each target of interest separately, and select the t clusters with the largest number, then a total of m×t clusters are selected from the m targets of interest;

[0020] Step S5.2: Take out the n clusters with the largest area among the above m×t clusters, and use the remaining m×t-n clusters as backup;

[0021] Step S5.3: respectively determine whether the distance between any two interest points in the n clusters taken out in step S5.2 is greater than a threshold distance; if so, set the center point of the n clusters as the interest point coordinate; if not, go to step S5.4;

[0022] Step S5.4: Delete the class that does not meet the distance requirement and has the least number of points in the cluster among the n-class clusters, and obtain the class with the largest number of points among the remaining m×t-n-class clusters in step S5.2 for replacement;

[0023] Step S5.5: Return to step S5.3 to continue judging;

[0024] Step S5.6: If the distances between any two interest points in the finally determined clusters are less than the threshold distance, the n-type cluster with the largest area is output.

[0025] Furthermore, t is a natural number less than or equal to 3, m is a natural number less than or equal to 10, n is a natural number less than or equal to 5, and the threshold distance range is 1m to 10m.

[0026] In summary, due to the adoption of the above technical solution, the beneficial effects of the present invention are:

[0027] (1) The method proposed in the present invention can divide the entire interest target library into several parts in chronological order after the patroller arrives at the target area, so as to ensure that the instantaneous error that may occur during the operation of the patroller will not affect the interest targets discovered before the error occurs.

[0028] (2) The discovery of the target of interest in the present invention is based on a clustering algorithm, so the accuracy required for each target of interest search can be appropriately relaxed, and ultimately the upper and lower color thresholds are used to establish a mask for target of interest discovery, which greatly reduces the time complexity and space complexity of the algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] In order to facilitate understanding by those skilled in the art, the present invention is further described below with reference to the accompanying drawings.

[0030] Figure 1 It is a flowchart of the interest point search and clustering of the present invention;

[0031] Figure 2 A schematic diagram of the transformation from a two-dimensional image coordinate system to a world coordinate system of the present invention;

[0032] Figure 3 It is a flow chart of transforming the two-dimensional image coordinate system to the world coordinate system of the present invention;

[0033] Figure 4 It is a schematic diagram of the patrol device coordinate system and the front and rear camera coordinate systems of the present invention;

[0034] Figure 5 A schematic diagram of the coordinate system conversion relationship of the patrol device of the present invention;

[0035] Figure 6 It is a schematic diagram of the DBSCAN (Density-Based Spatial Clustering of Applications with Noise) algorithm of the present invention. DETAILED DESCRIPTION

[0036] The technical solution of the present invention will be described clearly and completely in conjunction with the embodiments below. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. The embodiments of the present invention and all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0037] Step S1: Use a camera to obtain an environment image;

[0038] Step S2: Detecting color abnormal areas in the environment image, marking the color abnormal areas as interest points, and performing an average operation on the pixel areas of the environment image to obtain the pixel center coordinates of the interest points on the environment image;

[0039] Step S3: using the camera's intrinsic parameter matrix to perform mathematical operations, the pixel center coordinates of the interest point in the two-dimensional image coordinate system are converted into coordinates in the camera coordinate system, and according to the camera's intrinsic and extrinsic parameters, the obtained camera coordinates are subjected to corresponding matrix transformation and translation transformation, thereby obtaining the coordinates of the interest point in the patrol coordinate system;

[0040] Step S4: Obtain the coordinates of the interest point in the world coordinate system through the coordinates of the interest point in the patrol coordinate system, and store the coordinates of all the interest points found in the world coordinate system in the order of discovery in the interest target library. After the patroller reaches the target area and finishes patrolling, divide the entire interest target library into m interest targets in chronological order.

[0041] Step S5: cluster the m objects of interest respectively using the DBSCAN algorithm, and select the center point coordinates of the n clusters that meet the conditions as the coordinates of the points of interest.

[0042] Among them, the world coordinate system in step S4 is the northeast celestial coordinate system, which takes the starting point as the origin, the X-axis points to the north, the Y-axis points to the east, and the Z-axis points to the sky, and the three axes are mutually orthogonal to each other; wherein the rover coordinate system refers to a coordinate system with the center of mass of the rover as the origin, the X-axis points to the rover's forward direction, the Y-axis points to the left of the rover's forward direction, and the Z-axis points to the upward direction, and the three axes are mutually orthogonal to each other; wherein the camera coordinate system refers to a coordinate system with the camera aperture as the origin, the X-axis points to the left of the camera's shooting direction, the Y-axis points to the downward direction, and the Z-axis points to the forward direction, and the three axes are mutually orthogonal to each other; wherein the two-dimensional image coordinate system refers to a coordinate system with the upper left corner of the pixel image as the origin, the Y-axis points downward, and the X-axis points to the right, and the two axes are mutually orthogonal to each other.

[0043] Among them, in the S1, the camera adopts an RGB-D camera, which can output an RGB-D image; the RGB-D image is an image mode that combines an RGB (red, green, blue) color image and a depth image, through which rich environmental information can be obtained; among them, in the RGB-D image, the RGB image provides the x and y coordinates in the pixel coordinate system, and the depth image directly provides the Z coordinate in the camera coordinate system, that is, the distance between the camera and the observation point.

[0044] Wherein, the interest point in step S2 exists in a certain pixel area of ​​the environment image, and the pixel center coordinates of the interest point are the centroid pixel coordinates of the area.

[0045] The patroller in step S3 is equipped with a front RGB-D camera and a rear RGB-D camera, and there is a certain deflection angle between the axis of the camera lens and the horizontal direction of the patroller, and the deflection angle ranges from 0 to 180 degrees.

[0046] Among them, in step S3, the two-dimensional pixel image coordinate system can be mapped to the camera coordinate system to obtain the coordinates of each pixel point relative to the camera coordinate system; and since the position from the camera to the patroller is fixed, the installation matrix is ​​obtained according to the installation position of the camera on the patroller, and the coordinates of the point of interest in the patroller coordinate system can be obtained.

[0047] Among them, Figure 2 He Ru Figure 3 As shown, since the initial coordinates and posture of the patroller are known in the world coordinate system, and each step of the patroller's movement is recorded, the X, Y, and Z axis displacements of the patroller in the world coordinate system can be known by accumulating movement values; through inverse operations, the coordinates of the interest points relative to the patroller coordinate system obtained above can be converted into the coordinates of the interest points relative to the world coordinate system, that is, the accurate coordinates of the interest points are obtained.

[0048] In the preferred embodiment of the present invention, the resolution (w*h) of the camera is 1280*720, and the field of view FOV is 1.57 (in radians), so the formula can be combined to calculate:

[0049]

[0050] Among them, let the pixel center coordinates of the interest point on the environment image be (u, v) and the depth be z, then the coordinates of the interest point in the camera coordinate system are:

[0051]

[0052] Z c = z;

[0053] The coordinate system is then transformed through the installation matrix between the camera and the patroller, and the coordinates of the point of interest in the patroller coordinate system can be obtained.

[0054] like Figure 4 As shown in the figure, a group of RGB-D cameras are installed at the front and rear of the patroller, and the angle between the sight axes of the front and rear cameras and the horizontal is 20°. The front camera is tilted downward, and the rear camera is tilted upward. The center of the front and rear cameras is about 0.2m away from the center of the patroller in the horizontal direction.

[0055] According to the front camera's downward angle and the installation distance from the origin of the patroller's coordinate system, the coordinate system is rotated 110° counterclockwise around the positive direction of the x-axis, and then translated 0.2 meters along the negative direction of the y-axis to obtain the coordinates of the front camera coordinate system in the patroller's coordinate system. Among them, (θ1=110°, Δy1=-0.2), according to the rear camera's downward angle and the installation distance from the origin of the patroller's coordinate system, the rear camera coordinate system is first rotated 70° counterclockwise around the positive direction of the x-axis, then rotated 180° counterclockwise around the positive direction of the z-axis, and then translated 0.2 meters along the positive direction of the y-axis to obtain the coordinates of the rear camera coordinate system in the patroller's coordinate system. Among them, (θ2=70°, θ3=180°, Δy2=0.2).

[0056] Rotate the matrix by θ degrees counterclockwise around the x-axis:

[0057]

[0058] Rotate the rotation matrix by θ degrees counterclockwise around the z-axis:

[0059]

[0060] Translate the matrix in the positive direction:

[0061]

[0062] Therefore, the coordinates of the front camera's interest point in the patroller's coordinate system are:

[0063]

[0064] The coordinates of the rear camera's interest point in the patroller's coordinate system are:

[0065]

[0066] If the front camera interest point is in the patroller coordinate system coordinate (X a , Y a , Z a ) and the rear camera interest point in the patrol coordinate system Keep it consistent, that is, get the coordinates of the point of interest in the patrol coordinate system (X a , Y a , Z a ); if they are inconsistent, they need to be recalculated.

[0067] And because the starting point of the patrol is known in the world coordinate system, and the cumulative mileage of the patroller in each direction can be recorded, this information can be used to determine the coordinates of the point of interest in the world coordinate system. By translating the coordinates of the point of interest in the patroller coordinate system, the coordinates of the point of interest in the world coordinate system can be obtained. Finally, the target coordinates only need the xOy plane, so the z-axis coordinates are discarded here.

[0068] like Figure 5 As shown, let the starting coordinate of the patroller be (X o , Y o ), the displacement of the patroller along the x-axis and y-axis is (ΔX, ΔY), which is calculated by the formula:

[0069] (X, Y) = (X o , Y o )+(ΔX,ΔY)+(X a , Y a );

[0070] The coordinates (X, Y) of the point of interest in the world coordinate system can be obtained.

[0071] Among them, Figure 6 As shown in the figure, the DBSCAN algorithm is a density clustering algorithm used to group data points into clusters with similar density and can identify noise points. The core idea of ​​the algorithm is to use a set of parameters (∈, minPts) to describe the density of the data set, where ∈ is the spatial radius threshold of the selected point, and minPts is the minimum sample number threshold contained under the radius ∈.

[0072] Wherein, in the step S4, the coordinates of all the interest points in the world coordinate system are stored in the interest object library in the order of discovery, and the interest object library is divided into m interest objects in the order of discovery.

[0073] Among them, Figure 1 As shown, in step S5, the method for determining whether the cluster meets the conditions includes the following steps:

[0074] Step S5.1: cluster each target of interest separately, and select the t clusters with the largest number, then a total of m×t clusters are selected from the m targets of interest;

[0075] Step S5.2: Take out the n clusters with the largest area among the above m×t clusters, and use the remaining m×t-n clusters as backup;

[0076] Step S5.3: respectively determine whether the distance between any two interest points in the n clusters taken out in step S5.2 is greater than a threshold distance; if so, set the center point of the n clusters as the interest point coordinate; if not, go to step S5.4;

[0077] Step S5.4: Delete the class that does not meet the distance requirement and has the least number of points in the cluster among the n-class clusters, and obtain the class with the largest number of points among the remaining m×t-n-class clusters in step S5.2 for replacement;

[0078] Step S5.5: Return to step S5.3 to continue judging;

[0079] Step S5.6: If the distances between any two interest points in the finally determined clusters are less than the threshold distance, the n-type cluster with the largest area is output.

[0080] Among them, t is a natural number less than or equal to 3, m is a natural number less than or equal to 10, n is a natural number less than or equal to 5, the threshold distance range is 1m to 10m. In a preferred embodiment of the present invention, t is 3, m is 5, n is 5, and the threshold distance is 3m.

[0081] In summary, the present invention provides a method for searching points of interest for lunar patrol detection. After the patrol device reaches the target area, the entire library of targets of interest can be divided into several parts in chronological order to ensure that instantaneous errors that may occur during the operation of the patrol device will not affect the targets of interest discovered before the error occurs; and the discovery of targets of interest is based on a clustering algorithm, and the accuracy required for the search of targets of interest can be appropriately relaxed; and upper and lower color thresholds are used to establish a mask for the discovery of targets of interest, which greatly reduces the time complexity and space complexity of the algorithm.

[0082] The preferred embodiments of the present invention disclosed above are only used to help explain the present invention. The preferred embodiments do not describe all the details in detail, nor do they limit the invention to only specific implementation methods. Obviously, many modifications and changes can be made according to the content of this specification. This specification selects and specifically describes these embodiments in order to better explain the principles and practical applications of the present invention, so that those skilled in the art can understand and use the present invention well. The present invention is limited only by the claims and their full scope and equivalents.

Claims

1. A method for searching points of interest for a humanoid robot patrol mission, characterized in that: The following steps are involved: Step S1: Use a camera to obtain an environment image; Step S2: Detecting color abnormal areas in the environment image, marking the color abnormal areas as interest points, and performing an average operation on the pixel areas of the environment image to obtain the pixel center coordinates of the interest points on the environment image; Step S3: using the camera's intrinsic parameter matrix to perform mathematical operations, the pixel center coordinates of the interest point in the two-dimensional image coordinate system are converted into coordinates in the camera coordinate system, and according to the camera's intrinsic and extrinsic parameters, the obtained camera coordinates are subjected to corresponding matrix transformation and translation transformation, thereby obtaining the coordinates of the interest point in the patrol coordinate system; Step S4: Obtain the coordinates of the interest point in the world coordinate system through the coordinates of the interest point in the patrol coordinate system, and store the coordinates of all the interest points found in the world coordinate system in the order of discovery in the interest target library. After the patroller reaches the target area and finishes patrolling, divide the entire interest target library into m interest targets in chronological order. Step S5: cluster the m objects of interest respectively using the DBSCAN algorithm, and select the center point coordinates of the n clusters that meet the conditions as the coordinates of the points of interest.

2. A method for searching points of interest for a humanoid robot patrol mission according to claim 1, characterized in that: The world coordinate system in step S4 is a northeast celestial coordinate system, which is a three-axis mutually orthogonal coordinate system with the starting point as the origin, the X-axis pointing to the north, the Y-axis pointing to the east, and the Z-axis pointing to the sky; wherein the rover coordinate system refers to a three-axis mutually orthogonal coordinate system with the center of mass of the rover as the origin, the X-axis pointing to the rover's forward direction, the Y-axis pointing to the left of the rover's forward direction, and the Z-axis pointing to the upward direction; wherein the camera coordinate system refers to a three-axis mutually orthogonal coordinate system with the camera aperture as the origin, the X-axis pointing to the left of the camera's shooting direction, the Y-axis pointing to the downward direction, and the Z-axis pointing to the forward direction; wherein the two-dimensional image coordinate system refers to a two-axis mutually orthogonal coordinate system with the upper left corner of the pixel image as the origin, the Y-axis pointing downward, and the X-axis pointing to the right.

3. The method for searching points of interest for a humanoid robot patrol mission according to claim 1, characterized in that: The point of interest in step S2 exists in a certain pixel area of ​​the environment image, and the pixel center coordinates of the point of interest are the centroid pixel coordinates of the area.

4. The method for searching points of interest for a humanoid robot patrol mission according to claim 1, characterized in that: The patroller in step S3 is equipped with a front RGB-D camera and a rear RGB-D camera, and there is a certain deflection angle between the axis of the camera lens and the horizontal direction of the patroller, and the deflection angle ranges from 0 to 180 degrees.

5. The method for searching points of interest for a humanoid robot patrol mission according to claim 1, characterized in that: In step S3, the two-dimensional pixel image coordinate system can be mapped to the camera coordinate system to obtain the coordinates of each pixel point relative to the camera coordinate system; and since the position from the camera to the patroller is fixed, the installation matrix is ​​obtained according to the installation position of the camera on the patroller, and the coordinates of the point of interest in the patroller coordinate system can be obtained.

6. The method for searching points of interest for a humanoid robot patrol mission according to claim 5, characterized in that: The initial coordinates and posture of the patroller are known in the world coordinate system. The displacement of each axis of the patroller in the world coordinate system is known through the patroller's accumulated movement value, and then the coordinates of the interest point relative to the patroller coordinate system are converted into the coordinates of the interest point relative to the world coordinate system through inverse operation.

7. The method for searching points of interest for a humanoid robot patrol mission according to claim 1, characterized in that: In step S5, the method for determining whether the cluster meets the conditions includes the following steps: Step S5.1: cluster each target of interest separately, and select the t clusters with the largest number, then a total of m×t clusters are selected from the m targets of interest; Step S5.2: Take out the n clusters with the largest area among the above m×t clusters, and use the remaining m×t-n clusters as backup; Step S5.3: respectively determine whether the distance between any two interest points in the n clusters taken out in step S5.2 is greater than a threshold distance; if so, set the center point of the n clusters as the interest point coordinate; if not, go to step S5.4; Step S5.4: Delete the class that does not meet the distance requirement and has the least number of points in the cluster among the n-class clusters, and obtain the class with the largest number of points in the remaining m×t-n-class clusters in step S5.2 for replacement; Step S5.5: Return to step S5.3 to continue judging; Step S5.6: If the distances between any two interest points in the finally determined clusters are less than the threshold distance, the n-type cluster with the largest area is output. The method for searching points of interest for a humanoid robot patrol mission according to claim 7 is characterized in that: The t is a natural number less than or equal to 3, m is a natural number less than or equal to 10, n is a natural number less than or equal to 5, and the threshold distance range is 1m to 10m.

Images